#include <Bounce2.h>
#include <Servo.h>
/*

1. servo za hitrost spuscanja sklopke sa nastau ze u prvotnem loopu takoj ko on prebere gearup ali geardown
    posledicno je potrebna samo ena funkcija za spuscanje sklopke


*/
/*

const byte SEGMENT_LOOKUP[] = {
  B01110111,  // 0
  B00100100,  // 1
  B11100011,  // 2
  B11100110,  // 3
  B10110100,  // 4
  B11010110,  // 5
  B11010111,  // 6
  B01100100,  // 7
  B11110111,  // 8
  B11110110,  // 9
  B00001000,  // 10, .
  B01110000,  // 11, R
  B11010011,  // 12, E
  B00000000   // 13, blank

};

*/
#define clutchSwitchEnable 7
#define autoClutchEnable 12
#define clutchSensor 8
#define footSensor 6

#define gearUpPin 2
#define gearDownPin 4

#define cil_1 3
#define cil_2 5
#define cil_3 13
#define cil_4 9
#define clu 11
#define ger 10

#define latchPin 15
#define dataPin 16
#define clockPin 17

bool blip = false;

int cdel = 20; //isti gib samo da je v pravi poziciji
int bdel = 70; //da se premika po h vzorcu
int dispDelay = 100; //display blink

int gear = 0;
int gearN;
int gearO;
bool standby;
bool ready = false;  //ready da prestavi (sklopka pohojena)
bool done = false;  //gump je pritisnje v času

unsigned long startTime = 0;
unsigned long timeWindow = 2000;
unsigned long prevTime = millis();

Bounce2::Button gearUp = Bounce2::Button();
Bounce2::Button gearDown = Bounce2::Button();

Servo clutchValve;

void setup() {
  Serial.begin(9600);
  Serial.println("Start");

  clutchValve.attach(3);
  
  pinMode(cil_1, OUTPUT);
  pinMode(cil_2, OUTPUT);
  pinMode(cil_3, OUTPUT);
  pinMode(cil_4, OUTPUT);
  pinMode(clu, OUTPUT);
  pinMode(ger, OUTPUT);
  
  gearUp.attach(gearUpPin, INPUT);
  gearDown.attach(gearDownPin, INPUT);
  gearUp.interval(5); //debounce time ms
  gearDown.interval(5);
  gearUp.setPressedState(HIGH);
  gearDown.setPressedState(HIGH);
  
  /*pinMode(dataPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(latchPin, OUTPUT);*/

  pinMode(clutchSwitchEnable, INPUT_PULLUP);
  pinMode(autoClutchEnable, INPUT_PULLUP);
  pinMode(clutchSensor, INPUT_PULLUP);
  pinMode(footSensor, INPUT_PULLUP);

  standby = true;
  Serial.println("Standby");
  
  display(0);
  gear = 0;
  changeGear();
  
}

void loop() {

  ready = false;

  unsigned long currentTime = millis();

  if(currentTime - prevTime > 1000){
    Serial.println(gear);
    prevTime = currentTime;
  }

  gearUp.update();
  gearDown.update();

  if (gearUp.pressed() && gear <= 4) {   //read LOW becouse of pullup
    gearO = gear;
    gearN = gear + 1;
     delay(25);
    Serial.println("UP presses");
     /*Serial.println(gear);
     Serial.println(gearO);
     Serial.println(gearN);
     Serial.println(gearCheck);*/
  }else if (gearDown.pressed() && gear >= 0) {  //read LOW becouse of pullup FIX!!
    gearO = gear;
    gearN = gear - 1;
    delay(25);
    Serial.println("DOWN presses");
    /*Serial.println(gear);
    Serial.println(gearO);
    Serial.println(gearN);
    Serial.println(gearCheck);*/
  }
 
  if (gearN != gear){
    
    if(digitalRead(autoClutchEnable) == HIGH){
      Serial.println("Automatic clutch ENABLED");
      if(gear < gearN && gearN != -1 && gearN != 0 && gearN != 1){
        clutchValve.write(60); //hitreje spusca
        Serial.println("Servo 50 odprt");
      }
      if(gear > gearN && gearN != -1 && gearN != 0 && gearN != 1){
        clutchValve.write(120); //pocasneje spusca
        Serial.println("Servo 20 odprt");
      }
      if(gearN == -1 || gearN == 0 || gearN == 1 && gearO == 0){
        clutchValve.write(0); //čisto odprt
        Serial.println("Servo čisto odprt");
      }
      if(gearN == 1 && gearO == 2){
        clutchValve.write(30); //čisto odprt
        Serial.println("Servo 10 odprt");
      }
      clutchPre(); // tudi v primeru -1, 0, 1 ostane pritiskanje enako in vedno....spremeni se samo spuscanje
    }
    gear = gearN;
    check(); // -> ready ok/not
  }

  if(ready == true){
    Serial.println("READY");
    changeGear();
    ready = false;
    delay(500);
  }
}

void clutchPre(){
  
  done = false;
  digitalWrite(clu, HIGH);
  startTime = millis();
  while(millis() - startTime < timeWindow){
    if (digitalRead(clutchSensor) == HIGH) {                    //pullup?
    Serial.println("automatic clutch pressed in time");
      return;
      done = true;
    }
  }
  if(done != true){
    Serial.println("automatic clutch NOT pressed in time");
    error();
    done = false;
  }
}

void check(){
  Serial.println("Check");
  if (digitalRead(clutchSwitchEnable) == HIGH && done == false) {
    done = false;
    startTime = millis();
    while(millis() - startTime < timeWindow){
      if (digitalRead(clutchSensor) == HIGH) {                    //pullup?
        Serial.println("pressed in time");
        ready = true;
        return;
      }
    }
    if(done == false){
      Serial.println("not pressed in time");
      if(digitalRead(autoClutchEnable) == HIGH){
        error();
      }else{
        returnGear();
      }
      return;
    }
    
  }else {
    Serial.println("no need to press");
    ready = true;
    return;
  }
}

void clutchRel(){
  delay(100); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  if(digitalRead(autoClutchEnable) == HIGH){
    if(gear == 2 || gear == 3 || gear == 4 || gear == 5){
      Serial.println("Automatic clutch relesed");
      digitalWrite(clu, LOW);
    }
    if(gear == 1 && gearO == 2){
      Serial.println("Automatic clutch relesed slowly 2 -> 1");
      digitalWrite(clu, LOW);
    }
    if(gear == -1 || gear == 1 && gearO != 2){;
      int ch = 0;
      while(ch == 0){
        if(digitalRead(footSensor) == LOW){
          ch = 1;
        }
        delay(10);
      }
      Serial.println("Foot detected, automatic clutch relesed");
      digitalWrite(clu, LOW);
    }
    if(gear == 0){
      Serial.println("Automatic clutch still pressed");
    }
    //samo lucka
    //Serial.println("Automatic clutch released");
  }
}

/*void blip(){
  if(blipOn == true){

  }else{
    return;
  }
}*/

void error(){
  Serial.println("ERROR");   //Prisilni N
  delay(100000);
}

void returnGear(){
  Serial.println("Return gear");
  /*Serial.println((String) "O = " + gearO);
  Serial.println((String) "N = " + gearN);
  Serial.println((String) "G = " + gear);
  Serial.println((String) "C = " + gearCheck);*/
  gear = gearO;
  gearN = gearO;
  return;
}

void changeGear(){
  digitalWrite(ger, HIGH);
  switch(gear){
    case -1:
      if(gearO == 0){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("N -> R");
        display(6);
      }
    break;

    case 0:
      if(standby == true){

        standby = false;
        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("any -> N");
        display(0);

      }else if(gearO == -1 && standby == false){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("R -> N");
        display(0);

      }else if(gearO == 1 && standby == false){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("1 -> N");
        display(0);
      }
    break;

    case 1:
      if(gearO == 0){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("N -> 1");
        display(1);

      }else if(gearO == 2){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("2 -> 1");
        display(1);

      }
    break;

    case 2:
      if(gearO == 1){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("1 -> 2");
        display(2);

      }else if(gearO == 3){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("3 -> 2");
        display(2);

      }
    break;

    case 3:
      if(gearO == 2){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("2 -> 3");
        display(3);

      }else if(gearO == 4){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("4 -> 3");
        display(3);

      }
    break;

    case 4:
      if(gearO == 3){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("3 -> 4");
        display(4);

      }else if(gearO == 5){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("5 -> 4");
        display(4);

      }
    break;

    case 5:
      if(gearO == 4){

        digitalWrite(cil_1, HIGH);
        digitalWrite(cil_2, HIGH);
        digitalWrite(cil_3, HIGH);
        digitalWrite(cil_4, HIGH);
        Serial.println("4 -> 5");
        display(5);

      }
    break;
  }
  digitalWrite(ger, LOW);
  clutchRel();
}
 




void display(int digit) {
  /*if (digit < 0 || digit > 13) {
    return; // Exit 
  }
  byte segments = SEGMENT_LOOKUP[digit];
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, MSBFIRST, segments);
  digitalWrite(latchPin, HIGH);*/
  Serial.println((String) "Display: " + digit);
}