//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
//DWN  - if ON then decrease by 1kmh, if OFF then turns on to prev speed
//OFF   - Break light switch / clutch



#include <Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

Servo myservo;  // create servo object to control a servo

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int SetPoint = 0;
int Speed = 0;
int Output = 0;
int DispTime = 1;
unsigned long time_now = 0;
int DispTimeServo = 50;
unsigned long time_nowServo = 0;
bool PrevSW_ON=LOW;
bool PrevSW_DWN=LOW;
bool PrevSW_UP=LOW;
bool PrevSW_OFF=LOW;
bool Cruise = false;

void setup() {
  myservo.attach(9);
  pinMode(7, INPUT_PULLUP);
  pinMode(6, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);

  SetPoint = 0;

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  display.display();
  delay(1000); // Pause for 2 seconds
  display.clearDisplay();
  display.setTextColor(WHITE);
  
}

void loop() {
  ReadSpeed();
  ReadSW();
  RunServo();
  DisplayTFT();
}

void ReadSpeed(){
  Speed=map(analogRead(3),0,1023,0,180);
}

void DisplayTFT(){

    if(millis() -time_now >= DispTime){


  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 0);
  if(Cruise==true){
    display.println("Cruise ON");
  }
  if(Cruise==false){
    display.println("Cruise OFF");
  }
  display.print("Set: ");
  display.println(SetPoint);
  display.print("Pos: ");
  display.println(Output);
  display.print("Spd: ");
  display.println(Speed);
  display.display();

  time_now = millis();
  }
  
}

void RunServo(){
  if(Cruise == true){
    if(SetPoint>1){
      if(millis() -time_nowServo >= DispTimeServo){  
        if(SetPoint>Output)Output++;
        if(SetPoint<Output)Output--;
        myservo.write(Output);
        time_nowServo = millis();
      }
    }
  }
  if(Cruise == false){
    Output=0;
    myservo.write(Output);
  }
}

void ReadSW(){
  if(digitalRead(7)==LOW && PrevSW_ON == HIGH){
    switch (Cruise) {
      case true:
        Cruise=false;
        break;
      case false:
        SetPoint=0;
        Cruise=true;
        break;
      default:
        Cruise=false;
        break;
    }
  }
  PrevSW_ON = digitalRead(7);

  if(digitalRead(6)==LOW && PrevSW_UP == HIGH){
    if(Cruise == true){
      if (SetPoint<1){
        SetPoint=Speed;
      }
      SetPoint++;
    }
    if(Cruise == false){
      SetPoint=Speed;
      Cruise = true;
    }
  }
  PrevSW_UP = digitalRead(6);

  if(digitalRead(5)==LOW && PrevSW_DWN == HIGH){
    if(Cruise == true){
      SetPoint--;
    }
    if(Cruise == false){
      Cruise = true;
    }
  }
  PrevSW_DWN = digitalRead(5);

  if(digitalRead(4)==LOW && PrevSW_OFF == HIGH){
    Cruise = false;
  }
  PrevSW_OFF = digitalRead(4);
}