#include <Arduino.h>
#include <U8g2lib.h>
#include <EEPROM.h>
#include <ShiftRegister74HC595.h>
#include <HCSR04.h>

#define SR_BLOCKS_DIN 2
#define SR_BLOCKS_LATCH 4
#define SR_BLOCKS_CLK A1

#define SR_STRIP_DIN A3
#define SR_STRIP_LATCH A2
#define SR_STRIP_CLK A1

#define TRG0 A0
#define TRG1 12
#define ECHO0 13
#define ECHO1 11

#define US_L false
#define US_R true

#define US_DISTANCE_THRESHOLD 5

#define SWIPE_LEFT 1
#define SWIPE_RIGHT 2
#define SWIPE_NONE 0

#define PWM_GROUP_A 0
#define PWM_GROUP_B 1

#define STREET0 false
#define STREET1 true

#define STREET_COUNTER_MAX 100
#define BLOCK_COUNTER_MAX 10
#define SWITCH_COLOR_MAX 100
#define LCD_COUNTER_MAX 20
#define STRIP_COUNTER_MAX 8

#define ORANGE 0xFFB739
#define YELLOW 0xFF8000
#define PURPLE 0xCC00CC
#define WHITE  0xFFFFFF
#define CYAN   0x00FFFF

#define NUMBER_OF_STATES 5
#define STRIP_ONLY 0
#define SWITCHING_COLORS 1
#define STRIP_AND_RUNNING_RGB 2
#define ALL_ON 3
#define ALL_OFF 4

#define STRING_A "Quarantine is over! "
#define STRING_B "A lot of tickets    "
#define STRING_C "Greatest Offer Ever!"

static const uint32_t COLORS_ARRAY[] = { ORANGE, YELLOW, PURPLE, WHITE, CYAN };

static const uint8_t PWM_ARRAY[] = {2, 5, 6, 9, 10};
static const uint8_t STREET_ARRAY[] = {7, 8};

// Advertising strings //
static const char STRING_ARRAY[3][20] = { STRING_A, STRING_B, STRING_C};

// parameters: <number of shift registers> (data pin, clock pin, latch pin)
ShiftRegister74HC595<1> blocksSR(SR_BLOCKS_DIN, SR_BLOCKS_CLK, SR_BLOCKS_LATCH);
ShiftRegister74HC595<1> stripSR(SR_STRIP_DIN, SR_STRIP_CLK, SR_STRIP_LATCH);

UltraSonicDistanceSensor distanceSensorA(A0, 13);
UltraSonicDistanceSensor distanceSensorB(12, 11);

U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0);

uint8_t usSensors = SWIPE_NONE;
uint8_t currentDeviceMode;
uint8_t previousDeviceMode;

uint32_t stripCounter;
uint8_t stripIndex;

uint16_t blockCounter;
uint8_t blockValue;

uint16_t streetCounter;
bool street0Value;
bool street1Value;

uint16_t colorCounterA;
uint16_t colorCounterB;

uint32_t lcdCounter;
uint8_t lcdStringIndex;
bool lcdBlinkFlag;

uint16_t switchColorCounter;
uint8_t switchColorIndex;

void initIO() {
  pinMode(TRG0, OUTPUT);
  pinMode(TRG1, OUTPUT);
  pinMode(ECHO0, INPUT);
  pinMode(ECHO1, INPUT);
  for (uint8_t i = 0; i < 5; i++) pinMode(PWM_ARRAY[i], OUTPUT);
  for (uint8_t i = 0; i < 2; i++) pinMode(STREET_ARRAY[i], OUTPUT);
}

void setPwmValue(uint8_t number, uint8_t value) {
  analogWrite(PWM_ARRAY[number], value); 
}

uint8_t sampleUS() {
  if (distanceSensorA.measureDistanceCm() <= US_DISTANCE_THRESHOLD) {
    delay(1000);
    return SWIPE_RIGHT;
    }
  else if (distanceSensorB.measureDistanceCm() <= US_DISTANCE_THRESHOLD) {
    delay(1000);
   return SWIPE_LEFT;
  }
  return SWIPE_NONE;
}

void setStrip(uint8_t index) {
  if (index > 7) stripSR.setAllLow();
  else {
  for (uint8_t i = 0; i < 8; i++) {
    if (i == index) stripSR.set(i, HIGH); // set single pin HIGH
    else stripSR.set(i, LOW);
  }
  }
}

void setBlockLight(uint8_t block) {
  for (uint8_t i = 0; i < 8; i++) {
    if ((1 << i) & block) blocksSR.set(i, HIGH); // set single pin HIGH
    else blocksSR.set(i, LOW);
  }
}

void setStreet(bool street, bool val) {
  if (street == STREET0) digitalWrite(STREET_ARRAY[0], val);
  else digitalWrite(STREET_ARRAY[1], val);
}

void runningRGB(uint8_t group, uint16_t color) {
  uint8_t redIntensity;
  uint8_t greenIntensity;
  uint8_t blueIntensity;
        
  if (color <= 255) {
    redIntensity = 255 - color;    
    greenIntensity = color;       
    blueIntensity = 0;
  }
  else if (color <= 511) {
    redIntensity = 0;                     
    greenIntensity = 255 - (color - 256); 
    blueIntensity = (color - 256);
  }
  else {
    redIntensity = (color - 512);         
    greenIntensity = 0;                   
    blueIntensity = 255 - (color - 512);  
  }
  if (group == PWM_GROUP_A) {
    setPwmValue(0, 255 - greenIntensity);
    setPwmValue(1, 255 - blueIntensity);
  }
  else {
    setPwmValue(2, 255 - redIntensity);
    setPwmValue(3, 255 - greenIntensity);
    setPwmValue(4, 255 - blueIntensity);
  }
}

void setPwmColor(uint8_t group, uint32_t color) {
  uint8_t redColor = (color >> 16) & 0x0000FF;
  uint8_t greenColor = (color >> 8) & 0x0000FF;
  uint8_t blueColor = color & 0x0000FF;
  
  if (group == PWM_GROUP_A) {
    setPwmValue(0, 255 - greenColor);
    setPwmValue(1, 255 - blueColor);
  }
  else {
    setPwmValue(2, 255 - redColor);
    setPwmValue(3, 255 - greenColor);
    setPwmValue(4, 255 - blueColor);
  }
}

void u8g2_prepare(void) {
  u8g2.setFont(u8g2_font_logisoso22_tf);
  u8g2.setFontRefHeightExtendedText();
  u8g2.setDrawColor(1);
  u8g2.setFontPosTop();
  u8g2.setFontDirection(0);
}

void setup() {
  initIO();
  u8g2.begin();
  delay(300);
  u8g2_prepare();
  delay(300);
  initVariables();
  Serial.begin(9600);
}

void initVariables() {
  stripCounter = 0;
  stripIndex = 0x00;
  colorCounterA = 0;
  colorCounterB = 0;
  lcdCounter = 0;
  lcdStringIndex = 0;
  lcdBlinkFlag = 0;
  blockCounter = 0;
  blockValue = 0x00;
  streetCounter = 0;
  street0Value = false;
  street1Value = false;
  currentDeviceMode = STRIP_ONLY;
  previousDeviceMode = STRIP_ONLY;
  switchColorCounter = 0;
  switchColorIndex = 0;
}

void printLcd(uint8_t mode) {
  u8g2.setFont(u8g2_font_logisoso22_tf);
  u8g2.clearBuffer();

  switch(currentDeviceMode) {
    case STRIP_ONLY:
        u8g2.drawStr(0, 0, "Only Today!");
        break;

    case SWITCHING_COLORS:
        u8g2.drawStr(0, 0, "Grand Opening!");
        break;

    case STRIP_AND_RUNNING_RGB:
        u8g2.drawStr(0, 0, "Book Flight!");
        break;

    case ALL_ON:
        u8g2.drawStr(0, 0, "Cheap and Cozy!");
        break;      
         
    case ALL_OFF:
        u8g2.drawStr(0, 0, "No Limits!");
        break;      

      default: 
      break;
  }
    
  u8g2.sendBuffer();
}

void loop() {
  // Sample US Sensors
  usSensors = sampleUS();
  if(usSensors == SWIPE_RIGHT) {
    Serial.println("Swipe Right!");
    if (currentDeviceMode < NUMBER_OF_STATES) currentDeviceMode += 1;
    else currentDeviceMode = STRIP_ONLY;
    printLcd(currentDeviceMode);
  }
  if(usSensors == SWIPE_LEFT) {
    Serial.println("Swipe Left!");
    if (currentDeviceMode > 0) currentDeviceMode -= 1;
    else currentDeviceMode = ALL_OFF;
    printLcd(currentDeviceMode);
  }

  switch(currentDeviceMode) {
    case STRIP_ONLY:
        // Strip //
        setStrip(stripIndex);
        // PWM //
        setPwmColor(PWM_GROUP_A, 0);
        setPwmColor(PWM_GROUP_B, 0);
        // Street //
        setStreet(STREET0, LOW);
        setStreet(STREET1,LOW);
        // Block //
        setBlockLight(0x00);
        break;

    case SWITCHING_COLORS:
        // Strip //
        setStrip(stripIndex);
        // PWM //
        setPwmColor(PWM_GROUP_A, COLORS_ARRAY[switchColorIndex]);
        setPwmColor(PWM_GROUP_B, COLORS_ARRAY[4 - switchColorIndex]);
        // Street //
        setStreet(STREET0, LOW);
        setStreet(STREET1,LOW);
        // Block //
        setBlockLight(0x00);
        break;

    case STRIP_AND_RUNNING_RGB:
        // Strip //
        setStrip(stripIndex);
        // PWM //
        runningRGB(PWM_GROUP_A, colorCounterA);
        runningRGB(PWM_GROUP_B, colorCounterB);
        // Street //
        setStreet(STREET0, street0Value);
        setStreet(STREET1,street1Value);
        // Block //
        setBlockLight(0x00);
        break;

    case ALL_ON:
        // Strip //
        setStrip(stripIndex);
        // PWM //
        runningRGB(PWM_GROUP_A, colorCounterA);
        runningRGB(PWM_GROUP_B, colorCounterB);
        // Street //
        setStreet(STREET0, street0Value);
        setStreet(STREET1,street1Value);
        // Block //
        setBlockLight(blockValue);
        // LCD //
        //if (lcdBlinkFlag) printAdvertising(lcdStringIndex);
        //else u8g2.clearBuffer();
        break;      
         
    case ALL_OFF:
        // Strip //
        setStrip(8);
        // PWM //
        setPwmColor(PWM_GROUP_A, 0);
        setPwmColor(PWM_GROUP_B, 0);
        // Street //
        setStreet(STREET0, LOW);
        setStreet(STREET1,LOW);
        // Block //
        setBlockLight(0x00);
        // LCD //
        //u8g2.clearBuffer();
        break;      

      default: break;
  }

  /* Strip Counter */
  if (stripCounter >= STRIP_COUNTER_MAX) {
    stripCounter = 0;
    if (stripIndex < 8) stripIndex++;
    else stripIndex = 0;
  }
  else stripCounter++;

  /* Lcd Counter */
  if (lcdCounter < LCD_COUNTER_MAX) {
    lcdCounter++;
    lcdBlinkFlag = true;
  }
  else if (lcdCounter < LCD_COUNTER_MAX * 2) {
    lcdCounter++;
    lcdBlinkFlag = false;
  }
  else {
    if (lcdStringIndex < 3) lcdStringIndex++;
    else lcdStringIndex = 0;
    lcdCounter = 0;
  }
  
  /* PWM Counters */
  if (colorCounterA >= 767) colorCounterA = 0;
  else colorCounterA++;
  
  if (colorCounterB >= 767) colorCounterB = 0;
  else colorCounterB++;

  /* Switch Color Counters */
  if (switchColorCounter < SWITCH_COLOR_MAX) switchColorCounter++;
  else {
    switchColorCounter = 0;
    if (switchColorIndex < 4) switchColorIndex++;
    else switchColorIndex = 0;
  }

  /* Block Counter */
  if (blockCounter < BLOCK_COUNTER_MAX) {
    blockCounter++;
  }
  else {
    blockCounter = 0;
    if (blockValue < 0xFF) blockValue += 0x01;
    else blockValue = 0;
  }

  /* Street Counter */
  
  if (streetCounter < STREET_COUNTER_MAX) {
    streetCounter++;
    street0Value = LOW;
    street1Value = HIGH;
  }
  else if (streetCounter < STREET_COUNTER_MAX * 2) {
    streetCounter++;
    street0Value = HIGH;
    street1Value = LOW;
  }
  else if (streetCounter < STREET_COUNTER_MAX * 3) {
    streetCounter++;
    street0Value = HIGH;
    street1Value = HIGH;
  }
  else streetCounter = 0;
  previousDeviceMode = currentDeviceMode;
}