//ON    - Turns On Cruise if its off, turns it off if its on
//UP    - if ON then increase by 1kmh, if OFF then turns on to current speed, if off increase menu
//DWN  - if ON then decrease by 1kmh, if OFF then turns on to prev speed, if off back button
//OFF   - Break light switch / clutch



#include <Servo.h>
#include <Wire.h>
#include <AutoPID.h>
//#include <U8g2lib.h>
//#include <U8x8lib.h>

#include <SPI.h>
//#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

Servo myservo;  // create servo object to control a servo
//U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
//U8X8_SSD1306_128X64_NONAME_SW_I2C u8x8(/* clock=*/ A5, /* data=*/ A4, /* reset=*/ U8X8_PIN_NONE);
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//////////////////////////////////////////////////////////////////
/////////////////////////    SETTINGS    /////////////////////////
//////////////////////////////////////////////////////////////////

//pid settings and gains
#define KP 2
#define KI 0.5
#define KD 0.1
//Servo Motor Setup
int ServoMax = 100; // point at which servo is pulling max throttle
int ServoMin = 0; // point at which servo is pulling min throttle
//Cruise Enable Handover
int timerMax = 200;
//Throttle Reading Setup (1023 = 5V)
int ThrottleMin=0;//closed throttle reading
int ThrottleMax=1023;//max throttle reading
//Speed Setup
float PulseRev = 2.0; // make smaller to make cruise read higher than speedo
float WheelCirc = 1.997; //measured meters

/////////////////////////////////////////////////////////////////////////
/////////////////////////    Pin Assignments    /////////////////////////
/////////////////////////////////////////////////////////////////////////

const int On_Pin = 7;
const int Up_Pin = 6;
const int Down_Pin = 5;
const int Stop_Pin = 2;
const int Speed_Pin = 10;
const int Throt_Pin = A2;
const int Speed_Temp_Pin = A3;

///////////////////////////////////////////////////////////////////
/////////////////////////    Variables    /////////////////////////
///////////////////////////////////////////////////////////////////

#define OUTPUT_MIN 0
#define OUTPUT_MAX 100

int DispTime = 500;

double SetPoint = 0;
double Speed = 0;
double Output = 0;
unsigned long time_now = 0;
bool Cruise = false;
bool CruiseAllow = false;
int CurrThrottle;
int ServoPos;
int timer = 0;
bool MenuMode = false;

bool PrevSW_ON=LOW;
bool PrevSW_DWN=LOW;
bool PrevSW_UP=LOW;

AutoPID myPID(&Speed, &SetPoint, &Output, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);


void setup() {
  myservo.attach(9);
  myservo.write(ServoMin);
  pinMode(On_Pin, INPUT_PULLUP);
  pinMode(Up_Pin, INPUT_PULLUP);
  pinMode(Down_Pin, INPUT_PULLUP);
  
  pinMode(Speed_Pin, INPUT_PULLUP); //Speed Signal

  pinMode(Stop_Pin, INPUT);
  attachInterrupt(digitalPinToInterrupt(Stop_Pin), StopCruise, RISING);

  myPID.setBangBang(200);
  myPID.setTimeStep(50);

  SetPoint = 0;
  
  //u8g2.begin();
  //u8x8.begin();

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  // the library initializes this with an Adafruit splash screen.
  display.display();
  display.clearDisplay(); // Clear display buffer
}

void loop() {
  ReadSpeed();
  ReadSW();

  if(CruiseAllow==false){
    Cruise = false;
  }
  if(Cruise==true){
    myPID.run();
  }
  else{
    myPID.stop();
  }
  RunServo();
  DisplayTFT();
}

void ReadSpeed(){
   
  int tempSpeed = analogRead(Speed_Temp_Pin);
  Speed=map(tempSpeed,0,1023,0,200);
  /*
  float HighTime = pulseIn(10,HIGH);
  float LowTime = pulseIn(10,LOW);
  float Freq = 1000000 / ( HighTime + LowTime ); //pulse/sec
  
  Speed = Freq/PulseRev*3600*WheelCirc/1000;

  if (Speed < 0 || Speed > 300){
    Speed = 0;
  }
  */

  if (Cruise == true) {
    if ((Speed - SetPoint)>20){
      StopCruise();
    }
  }

  if (timer < 2){
    int tempThrottle = analogRead(Throt_Pin);
    CurrThrottle=map(tempThrottle,ThrottleMin,ThrottleMax,0,100);
  }
}

void DisplayTFT(){
  /*u8g2.firstPage();
  do {
    MainScreen();
    } while ( u8g2.nextPage() );*/

  if(millis() -time_now >= DispTime){
    MainScreen();
    time_now = millis();
  }

}

void MainScreen(){    
    /*u8g2.setFont(u8g2_font_fub11_tf);	// 8 Height
    if(!CruiseAllow){
      u8g2.setCursor(25, 12);
      u8g2.print("DISABLED");		// "Hello World" 
    }
    if(CruiseAllow && !Cruise){
      u8g2.setCursor(35, 12);
      u8g2.print("READY");		// "Hello World" 
    }
    if(CruiseAllow && Cruise){
      u8g2.setCursor(25, 12);
      u8g2.print("ENGAGED");		// "Hello World" 
    }
    
    u8g2.setFont(u8g2_font_fub11_tf);
    u8g2.setCursor(12, 29);
    u8g2.print("KPH");
    u8g2.setFont(u8g2_font_fub25_tf);	// 11 Height
    u8g2.setCursor(55, 40); 
    u8g2.print(int(Speed));
    u8g2.setFont(u8g2_font_fub11_tf);
    u8g2.setCursor(12, 58);
    u8g2.print("SET");
    u8g2.setFont(u8g2_font_fub20_tf);	// 11 Height	
    u8g2.setCursor(55, 64); 
    u8g2.print(int(SetPoint));	*/

  //u8x8.clear();
  /*u8x8.setFont(u8x8_font_8x13B_1x2_f);
  if(!CruiseAllow){
      u8x8.drawString(0,0," DISABLED     ");
    }
    if(CruiseAllow && !Cruise){
      u8x8.drawString(0,0," READY      ");
    }
    if(CruiseAllow && Cruise){
      u8x8.drawString(0,0," ENGAGED     ");
    }
     
  u8x8.setFont(u8x8_font_profont29_2x3_f);
  if (Speed<100){
    u8x8.drawString(7,2," ");
    u8x8.setCursor(9,2);
    u8x8.print(int(Speed));
  }
  else{
    u8x8.setCursor(7,2);
    u8x8.print(int(Speed));
  }
  if (SetPoint<100){
    u8x8.drawString(7,5," ");
    u8x8.setCursor(9,5);
    u8x8.print(int(SetPoint));
  }
  else{
    u8x8.setCursor(7,5);
    u8x8.print(int(SetPoint));
  }*/
  //display.clearDisplay(); // Clear display buffer
  
  display.clearDisplay(); // Clear display buffer
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  if(!CruiseAllow){
      display.print("DISABLED     ");
  }
  else{
    if(!Cruise){
      display.print("READY      ");
    }
    if(Cruise){
      display.print("ENGAGED     ");
    }
  }

  display.setTextSize(2);
  display.setCursor(20,17);
  display.print("SPD");
  display.setTextSize(3);
  display.setCursor(60,17);
  display.print(int(Speed));
  
  display.setTextSize(2);
  display.setCursor(20,40);
  display.print("SPD");
  display.setTextSize(3);
  display.setCursor(60,40);
  display.print(int(Speed));

  display.display();
}


void RunServo(){
  if(Cruise == true){
    if(timer<timerMax && timer >=0){
      ServoPos=map(map(timer,0,timerMax,CurrThrottle,Output),0,100,ServoMin,ServoMax);
      myservo.write(ServoPos);
      timer++;
    }
    else{
      ServoPos=map(Output,OUTPUT_MIN,OUTPUT_MAX,ServoMin,ServoMax);
      myservo.write(ServoPos);
    }
  }
  if(Cruise == false){
    ServoPos=map(0,0,100,ServoMin,ServoMax);
    myservo.write(ServoMin);
  }
}

void ReadSW(){


  if(digitalRead(On_Pin)==LOW && PrevSW_ON == HIGH){
    if(MenuMode==false){
      switch (CruiseAllow) {
        case true:
          //SetPoint=0;
          CruiseAllow=false;
          break;
        case false:
          //SetPoint=0;
          CruiseAllow=true;
          break;
        default:
          //SetPoint=0;
          CruiseAllow=false;
          break;
      }
    }
  }
  PrevSW_ON = digitalRead(On_Pin);


    if(digitalRead(Up_Pin)==LOW && PrevSW_UP == HIGH){
      if(Cruise == true && CruiseAllow==true ){
        if(Speed>=(SetPoint+10)){
          SetPoint=Speed;
          timer = 0;
        }
        else{
          SetPoint++;
        }
      }
      if(Cruise == false && CruiseAllow==true){
        if(Speed>=30){
          SetPoint=Speed;
          timer = 0;
          Cruise = true;
        }
      }
      if(CruiseAllow==false){
        //Menu Increase
      }
    }
    PrevSW_UP = digitalRead(Up_Pin);

    if(digitalRead(Down_Pin)==LOW && PrevSW_DWN == HIGH){
      if(Cruise == true && CruiseAllow==true){
        SetPoint--;
        if(SetPoint<30){
          SetPoint=30;
        }
      }
      if(Cruise == false && Speed>=30 && CruiseAllow==true && SetPoint > 0){
        timer = 0;
        Cruise = true;
      }
      if(CruiseAllow==false){
        //Menu Decrease
      }
    }
    PrevSW_DWN = digitalRead(Down_Pin);

}

void StopCruise(){
  Cruise = false;
  myservo.write(ServoMin);
  Output = 0;
  timer = 0;
}