#include <Servo.h>

#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
bool usesAdafruit = false;

#define buttonPin0 4
#define buttonPin1 3
#define ledPin0 5 
#define ledPin1 6 
#define ledPin2 7
#define ledPin3 8
#define ledPin4 9
#define ledPin5 10
#define interval 500

#define servoPin 2
#define debounceDelay 50

Servo myServo;

int ledPins[] = {ledPin0,ledPin3,ledPin1,ledPin4,ledPin2,ledPin5};
bool ledStates[6] = {LOW,LOW,LOW,LOW,LOW,LOW};
int8_t ledIndex = 0;

bool lastButtonState0 = false;
unsigned long lastDebounceTime0 = 0;
bool buttonState0 = false;

bool lastButtonState1 = false;
unsigned long lastDebounceTime1 = 0;
bool buttonState1 = false;

bool start = false;
bool stop = false;
bool startTimer = false;
bool isRunning = false;

unsigned long previousMillis = 0;

unsigned long servoStartTime = 0;

int8_t rocketPos = -1;

byte rocketParts[8][8] = {
  { //spic
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b10000,
    0b11000,
    0b11100
  },
  { //spic
    0b11100,
    0b11000,
    0b10000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000
  },
  { //spodok
    0b00000,
    0b00000,
    0b11100,
    0b11110,
    0b11111,
    0b11111,
    0b11111,
    0b11111
  },
  { //spodok
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11110,
    0b11100,
    0b00000,
    0b00000
  },
  { //trup
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b11111,
    0b11111,
    0b11111
  },
  { //trup
    0b11111,
    0b11111,
    0b11111,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000
  },
  { //ohen
    0b00000,
    0b00000,
    0b00111,
    0b01101,
    0b11001,
    0b11001,
    0b01101,
    0b00111	  
  },
  { //ohen
    0b00111,
    0b01101,
    0b11001,
    0b11001,
    0b01101,
    0b00111,
    0b00000,
    0b00000
  }
};

void setup() {
  for(int i=0;i<(sizeof(ledPins)/sizeof(int));i++){
    pinMode(ledPins[i], OUTPUT);
    digitalWrite(ledPins[i], ledStates[i]);
  }

  pinMode(buttonPin0, INPUT);
  pinMode(buttonPin1, INPUT);

  myServo.attach(servoPin);
  myServo.write(180);

  if(usesAdafruit){
    lcd.begin(16,2);
    lcd.setBacklight(HIGH);
  }
  else{
    lcd.init();
    lcd.backlight();
  }

  for(int i=0;i<8;i++){
    lcd.createChar(i, rocketParts[i]);
  }
  zobrazRaketu();
  //zobraz("Hello");
  delay(1000);
}

void loop() {
  //button handiling
  if(debounceButton(buttonPin0, lastButtonState0, lastDebounceTime0, buttonState0)){
    start = true;
    stop = false;
    if(!startTimer){
      servoStartTime = millis();
      isRunning = true;
      rocketPos = -1;
      zobrazRaketu();
      }
    startTimer = true;
  }
  if(debounceButton(buttonPin1, lastButtonState1, lastDebounceTime1, buttonState1)){
    stop = true;
    start = false;
    startTimer = false;
  }

  //LED pattern
  if(millis()-previousMillis>=interval && start){
    previousMillis = millis();
    ledBlink(ledPins[ledIndex],ledStates[ledIndex]);
    ledBlink(ledPins[ledIndex+1],ledStates[ledIndex+1]);
    ledIndex=(ledIndex+2)%6;
    if(!isRunning){
      zobrazRaketu();
    }
  }

  //servo timer
  if(startTimer && isRunning){
    servoTimer();
  }

  //zobraz();
}

//int8_t rocketPos = -1;

void zobrazRaketu(){
  lcd.clear();
  lcd.setCursor(rocketPos,0);
  if(rocketPos >= 0) lcd.write(6);
  if(rocketPos >=-1) lcd.write(2);
  if(rocketPos >=-2) lcd.write(4);
  if(rocketPos >=-3) lcd.write(4);
  if(rocketPos >=-4) lcd.write(0);
  lcd.setCursor(rocketPos,1);
  lcd.write(7);
  lcd.write(3);
  lcd.write(5);
  lcd.write(5);
  lcd.write(1);

  rocketPos++;
  if(rocketPos >= 16){
    rocketPos = -5;
  }
}

void zobraz(){
  lcd.setCursor(0,0);
  lcd.print("start ");
  lcd.print(start);

  lcd.setCursor(0,1);
  lcd.print("stop  ");
  lcd.print(stop);
}

void zobraz(String text){
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(text);
}

bool debounceButton(int8_t buttonPin,bool& lastButtonState, unsigned long& lastDebounceTime, bool& buttonState){
  
  bool reading = digitalRead(buttonPin);
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == HIGH) {
        lastButtonState = reading;
        return true;
      }
    }
  }
  lastButtonState = reading;

  return false;
}

void ledBlink(int8_t ledPin, bool& ledState){
  ledState=!ledState;
  digitalWrite(ledPin,ledState);
}

  uint8_t uhol;

void servoTimer(){
  unsigned long progress = millis() - servoStartTime;
  if(progress/1000 <= 10)
    uhol = map(progress/1000,0,10,180,0);
  else{
    isRunning = false;
    uhol = 0;
  }
  myServo.write(uhol);
}