#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <Key.h>
#include <Keypad.h>

class InvertS {
  public:
    int s;
    int8_t invert = 1;
    int speed = 100;
    InvertS(int p_s = 0) {
      s = p_s;
    }

    void update_s() {
      if (millis() - time >= speed) {
        time = millis();
        if (s >= verh) {
          invert = -1;
        }
        else if (s <= niz) {
          invert = 1;
        }

        s += invert;
      }
    }
  private:
    unsigned long time = millis();
    byte verh = 255;
    byte niz = 0;
};

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define swRGBpin 43
#define swRadPin 39
#define zamerPin 53

bool swLCD;

long radius = 500;  // радиус колеса в мм
long inputRadius = 0;
double dlina_kolesa() {
  return 2 * radius * 3.1415926535;
}
uint64_t time = 0;
const int maxTime = 1000;
double speed;
double rasst = 0;

byte readState;
byte lastReadState;
uint32_t lastTimeRead;
const byte maxTimeRead = 5;
bool reaD;

const byte ROWS = 4;
const byte COLS = 4;
char hexaKeys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {23, 25, 27, 29};
byte colPins[COLS] = {31, 33, 35, 37};
Keypad cusKeyp = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);
byte s_key = 0;

const byte RGBpins[3][3] = {
  {10, 11, 12},
  {7, 6, 9},
  {4, 3, 5}
};
InvertS RGB[3] = {InvertS(255), InvertS(0), InvertS(0)};
InvertS RGB32[3][32];

bool newRGBstate = false;
bool RGBon = false;
byte RGBstate = 0;
uint64_t s = 0;
const int timer = 1000;

void setup() {
  for (byte j {0}; j < 3; j++) {
    for (byte i {0}; i < 3; i++) {
      pinMode(RGBpins[j][i], OUTPUT);
    }
  }
  pinMode(swRGBpin, INPUT_PULLUP);
  pinMode(swRadPin, INPUT_PULLUP);
  pinMode(zamerPin, INPUT_PULLUP);

  lcd.init();
  lcd.backlight();

  if(digitalRead(swRadPin) == HIGH) {
    swLCD = true;
  }
  else {
    swLCD = false;
  }
  update_lcd();

  createRGB32();
  
}

void loop() {
  if (digitalRead(swRGBpin) == HIGH) {
    RGBon = true;
    pressABCDkey();
  }
  else {    
    clearRGBpin();
    RGBon = false;
  }
  
  writeRGBpin();
  update_lcd();
  writeRGBpin();
}

void createRGB32() {
  byte a = 255;
  for (byte i {0}; i < 32; i++) {  
    RGB32[0][i] = InvertS(a);
    RGB32[0][i].speed = 50;
    a -= 7;
  }

  a = 255;
  byte invert = 1;
  for (byte i {0}; i < 32; i++) {  
    RGB32[1][i] = InvertS(a);
    RGB32[1][i].speed = 200;
    if(RGB32[1][i].s < 40) {
      invert = -1;
    }
    else if(RGB32[1][i].s == 255) {
      invert = 1;
    }
    a -= 14 * invert;
  }

  a = 255;
  for (byte i {0}; i < 32; i++) {  
    RGB32[2][i] = InvertS(a);
    RGB32[2][i].speed = 200;
    if(RGB32[2][i].s < 60) {
      invert = -1;
    }
    else if(RGB32[2][i].s == 255) {
      invert = 1;
    }
    a -= 28 * invert;
  }
}

void createRGB32D() {
  for(byte i {0}; i < 3; i++) {
    byte a = 0;
    for(byte j {0}; j < 32; j++) {
      RGB32[i][j] = InvertS(a);
      if(j % 2 != 0) {
        a += 14;
      }
    }
  }
}

void print_to_lcd(byte x, byte y, String stroka) {
  lcd.setCursor(x, y);
  lcd.print(stroka);
}

void update_lcd() {
  if (digitalRead(swRadPin) == HIGH) {
    writeRGBpin();
    if (swLCD) {
      time = millis();
      swLCD = false;
      lcd.clear();
      print_to_lcd(0, 0, "Distance: ");
      print_to_lcd(0, 1, String(rasst) + " km");
      print_to_lcd(0, 2, "Speed: ");
      print_to_lcd(0, 3, "0 km/h");
    }
    writeRGBpin();
    update_speed();
    writeRGBpin();
  }
  else {
    writeRGBpin();
    if (!swLCD) {
      swLCD = true;
      lcd.clear();
      print_to_lcd(0, 0, "CurRadius: ");
      print_to_lcd(0, 1, String(radius) + " mm");
      print_to_lcd(0, 2, "NewRadius: ");
      print_to_lcd(0, 3, "0 mm");
    }
    writeRGBpin();
    pressKey();
    writeRGBpin();
  }
}

void writeRGBpin() {
  if(RGBon) {
    if(RGBstate == 0) {
      stateA();
      for (byte i {0}; i < 3; i++) {
        digitalWrite(RGBpins[i][0], LOW);

        digitalWrite(RGBpins[i][1], LOW);
        analogWrite(RGBpins[i][2], RGB[i].s);
        digitalWrite(RGBpins[i][1], HIGH);

        digitalWrite(RGBpins[i][0], HIGH);
      }
    }
    else if(RGBstate == 1) {
      stateB();
      writeRGB32();
    }
    else if(RGBstate == 2) {
      stateC();
      writeRGB32();
    }
    else if(RGBstate == 3) {
      stateD();
      writeRGB32();
    }

  }
}

void writeRGB32() {
  for(byte i {0}; i < 3; i++) {
    digitalWrite(RGBpins[i][0], LOW);
    for(byte j {0}; j < 32; j++) {
      digitalWrite(RGBpins[i][1], LOW);
      analogWrite(RGBpins[i][2], RGB32[i][j].s);
      digitalWrite(RGBpins[i][1], HIGH);
      delayMicroseconds(199);
    }
    digitalWrite(RGBpins[i][0], HIGH);
  }
}

void clearRGBpin() {
  for (byte i {0}; i < 3; i++) {
    digitalWrite(RGBpins[i][0], LOW);

    digitalWrite(RGBpins[i][1], LOW);
    analogWrite(RGBpins[i][2], 0);
    digitalWrite(RGBpins[i][1], HIGH);

    digitalWrite(RGBpins[i][0], HIGH);
  }
}

void newState(byte r, byte g, byte b, byte speed) {
  if(newRGBstate) {
    RGB[0].s = r;
    RGB[1].s = g;
    RGB[2].s = b;
    newRGBstate = false;
  }
  for(byte i {0}; i < 3; i++) {
    RGB[i].speed = speed;
  }
}

void stateA() {
  newState(255, 0, 0, 10);

  for(byte i {0}; i < 3; i++) {
    byte j = i + 1;
    if(j > 2) {
      j = 0;
    }
    byte k = j + 1;
    if(k > 2) {
      k = 0;
    }

    if(RGB[i].s > 0 && RGB[k].s == 0) {
      if(RGB[i].s > 200 && RGB[j].s == 0) {
        RGB[i].update_s();
      }
      else {
        RGB[j].update_s();
        if(RGB[j].s <= 200) {
          RGB[i].update_s();
        }
        else {
          RGB[i].s = 0;
        }
      }
      break;
    }
  }
}

void stateB() {
  if(newRGBstate) {
    createRGB32();
    newRGBstate = false;
  }
  for (byte j {0}; j < 3; j++) {
    for (byte i {0}; i < 32; i++) {
      RGB32[j][i].update_s();
    }
  }
}

void stateC() {
  if(newRGBstate) {
    newRGBstate = false;
    for (byte j {0}; j < 3; j++) {
      for (byte i {0}; i < 32; i++) {
        RGB32[j][i].speed = 25;
        RGB32[j][i].s = random(0, 255);        
      }
    }
  }

  bool rand = false;
  if(millis() - s >= timer) {
    s = millis();
    rand = true;
  }
  for (byte j {0}; j < 3; j++) {
    for (byte i {0}; i < 32; i++) {
      if(rand) {
        RGB32[j][i].s = random(10, 255);
      }
      RGB32[j][i].update_s();
    }
  }
  if(rand) {
    rand = false;
  }
}

void stateD() {
  if(newRGBstate) {
    newRGBstate = false;
    createRGB32D();
  }

  for (byte j {0}; j < 3; j++) {
    for (byte i {0}; i < 32; i++) {
      RGB32[j][i].update_s();
    }
  }
}

void checkBut() {
  readState = digitalRead(zamerPin);
  if (readState != lastReadState) {
    lastReadState = readState;
    if (readState == LOW) {
      if (millis() - lastTimeRead > maxTimeRead) {
        reaD = true;
        lastTimeRead = millis();
      }
    }
  }
}

void update_speed() {
  checkBut();
  if (millis() - time >= maxTime && speed != 0) {
    time = millis();
    speed = 0;
    print_to_lcd(0, 3, "0 km/h              ");
  }
  else {
    if (reaD) {
      reaD = false;
      double time_ob = (millis() - time);
      speed = dlina_kolesa() / time_ob * 3.6;
      rasst += dlina_kolesa() / 1000000;
      print_to_lcd(0, 1, String(rasst) + " km");
      print_to_lcd(0, 3, String(speed) + " km/h    ");
      time = millis();
    }
  }
}

void pressKey() {
  char cuKey = cusKeyp.getKey();

   if(cuKey) {
    if(cuKey == '*') {
      if(s_key > 0) {
        s_key -= 1;
        print_to_lcd(14, 2, "    ");
        print_to_lcd(14, 3, "      ");
        print_to_lcd(0, 3, String(valueKeyMinus()) + " mm    ");
      }
    }
    else if(cuKey == '#') {
      radius = inputRadius;
      inputRadius = 0;
      s_key = 0;
      print_to_lcd(14, 2, "    ");
      print_to_lcd(14, 3, "      ");
      print_to_lcd(0, 1, String(radius) + " mm          ");
      print_to_lcd(0, 3, "0 mm          ");
    }
    else if(cuKey == 'A') {
      RGBstate = 0;
      newRGBstate = true;
    }
    else if(cuKey == 'B') {
      RGBstate = 1;
      newRGBstate = true;
    }
    else if(cuKey == 'C') {
      RGBstate = 2;
      newRGBstate = true;
    }
    else if(cuKey == 'D') {
      RGBstate = 3;
      newRGBstate = true;
    }
    else {
      if(s_key < 9) {
        print_to_lcd(0, 3, String(valueKeyPlus(cuKey)) + " mm    ");
        if(inputRadius > 0) {
          s_key += 1;
        }
      }
      else {
        print_to_lcd(14, 2, "MAX");
        print_to_lcd(14, 3, "RADIUS");
      }
    }
  }
}

void pressABCDkey() {
  char cuKey = cusKeyp.getKey();

   if(cuKey) {
    if(cuKey == 'A') {
      RGBstate = 0;
      newRGBstate = true;
    }
    else if(cuKey == 'B') {
      RGBstate = 1;
      newRGBstate = true;
    }
    else if(cuKey == 'C') {
      RGBstate = 2;
      newRGBstate = true;
    }
    else if(cuKey == 'D') {
      RGBstate = 3;
      newRGBstate = true;
    }
  }
}

long valueKeyPlus(char val) {
  byte a = val;
  a -= 48;
  if(inputRadius == 0) {
    inputRadius += a;
  }
  else{
    inputRadius *= 10;
    inputRadius += a ;
  }

  return inputRadius;
}

long valueKeyMinus() {
  inputRadius -= inputRadius % 10;
  inputRadius /= 10;

  return inputRadius;
}