#include <ESP32Servo.h>  // model servo: DS3218 PRO
#include <SPI.h>
#include <Wire.h>
#include <time.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define Version "2.2.10"
////2DO:
// menu do zmiany zakresu predkosci biegów
// menu do zmiany zakresu kątów biegów, obwodu koła, ilosci magnesow

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// #define PinInterrupt 0       //on board: RX deklaracja pod stabilizacje TX
#define PinInSpeed 34        //on board: D34
#define ServoSwitch 15       //on board: D15
#define ServoPin 19          //on board: D19
#define BrakingLight 13      //on board: D13    BrakingLight
#define PinLED 25           //on board: D25   ORANGE loop signal
#define VoltInptPin 27             //on board: D27   
#define Btn2 33             //on board: D33   Button2
#define ServoShift 4      //ręczna kalibracja biegu
// SDA D21
// SCL D22
// #define VoltInptPin 23      //on board: D23   Battery Voltage    prawdopodobnie uszkodzone ADC

#define ServoMaxAngle 130
#define MaxAngle 179
#define MinAngle 1
#define MaxGear 8
#define MinGear 1
#define WheelCircumference  2.130
#define MagnetsCnt 8
#define ms2kmh 3.6
#define Pi 3.1416
#define TimeToSleepMs 5000 //5 sec
#define LongTimeToSleepMs 150000 //150 sec
#define GearDelayMs 1500
Servo myservo;

//SPEED
double readSignalTime_5;
double readSignalTime_4;
double readSignalTime_3;
double readSignalTime_2;
double readSignalTime_1;
double readSignalTimeAvg;
double raw_speed;
double calcSpeedMain;
double calcSpeed3;
double calcSpeed2;
double calcSpeed1;
double calcSpeedAvg;

int speedTrend = 0;
double readSignalTime = 0.0;
double sleepSpd = 0.0;
unsigned long millissSpd = millis();
unsigned long lastMillisSpd = millis();
unsigned long lastLastMillisSpd = millis();
unsigned long loopTime = millis();
//GEAR
int currentGear = 1;
int calculatedGear = 1;
//Przedziały dia biegów
float spdRange1and2 = 8.5;
float spdRange2and3 = 13.0;
float spdRange3and4 = 16.5;
float spdRange4and5 = 18.0;
float spdRange5and6 = 24.5;
float spdRange6and7 = 29.9;
float spdRange7and8 = 36.5;
double calcTimeDiff = 0.0;
double lastGearCalc = millis();
double speedDiffKmh = 0.80;
double accelerationShift = 1.0;
int displGear = 9 - currentGear;
float currentGearRangeLower = 0;
float currentGearRangeUpper = 7.5;
//SERVO
int pos = 0;
int sleepMode = 0;
int servoCurrPos = ServoMaxAngle;
//GearRangePointer
int pointerPosition = 0;
unsigned int pointerVisibility = 1;
//BATTERY
float referenceVoltage = 3.3;
int maxADCValue = 4095;
float  voltageDividerRatio = 5.95;
int adcBattVoltValue = 0;
float inputVoltage = 0.0;
float measuredVoltage  = 0.0;
int voltBarHeight = 0;
int voltBarPosition = 0;
//oth
int BrakingLightSwitch;
int run_hrs = 0;
int run_mins = 0;
void setup() {
  //SERVO
  digitalWrite(ServoSwitch, HIGH);
  myservo.attach(ServoPin); // attaches the servo on pin 4 to the servo object
  setPosition(8);

  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setRotation(0);
  display.setTextSize(3);
  display.setCursor(0, 0);
  display.println("Version:");
  display.setCursor(0, 25);
  display.println(Version);
  display.display();
  delay(500);
  //INPUT
  pinMode(PinInSpeed, INPUT);
  // pinMode(PinInterrupt, OUTPUT);
  // digitalWrite(PinInterrupt, HIGH);
  pinMode(VoltInptPin, INPUT);
  pinMode(Btn2, INPUT);

  
  //OUTPUT
  pinMode(PinLED, OUTPUT);
  pinMode(BrakingLight, OUTPUT);
  pinMode(ServoSwitch, OUTPUT);

  //Interrupts
  attachInterrupt(digitalPinToInterrupt(PinInSpeed), readSpeed, FALLING);
  display.clearDisplay();
  delay(550);
  setPosition(7);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("wheelSize:");
  display.setCursor(75, 0);
  display.println(WheelCircumference);
  display.setCursor(0, 16);
  display.println("MagnetsCnt:");
  display.setCursor(75, 16);
  display.println(MagnetsCnt);
  display.setCursor(0, 30);
  display.println("TimeToSleepMs:");
  display.setCursor(75, 30);
  display.println(TimeToSleepMs);
  display.setCursor(0, 45);
  display.println("ServoMaxAgl:");
  display.setCursor(75, 45);
  display.println(ServoMaxAngle);
  display.display();
  digitalWrite(ServoSwitch, HIGH);
  for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
    myservo.write(servoCurrPos);
    delay(15);
  }
}


void setPosition(int currentGear) {
  pos = 180 - round((currentGear - 1) * (ServoMaxAngle / (MaxGear - 1) ));
  if (pos >= 180) {
    pos = MaxAngle;
  }

  if (pos <= 0) {
    pos = MinAngle;
  }
  if (sleepMode == 1 && calcSpeedMain > 0.0) {
    digitalWrite(ServoSwitch, HIGH);
    for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
      myservo.write(servoCurrPos);
      delay(4);
    }
    sleepMode = 0;
  }
  pos = pos + ServoShift; //reczna kalibracja
  myservo.write(pos);
}

void readSpeed() {
  lastLastMillisSpd = lastMillisSpd;
  lastMillisSpd = millissSpd;
  millissSpd = millis();
  readSignalTime_5 = readSignalTime_4;
  readSignalTime_4 = readSignalTime_3;
  readSignalTime_3 = readSignalTime_2;
  readSignalTime_2 = readSignalTime_1;
  readSignalTime_1 = double(millissSpd - lastLastMillisSpd) / 1000;
  readSignalTimeAvg = (readSignalTime_1 + readSignalTime_2 + readSignalTime_3 )/3;

  raw_speed  = (((2 * Pi) / readSignalTimeAvg * ((WheelCircumference) / (Pi)) * ms2kmh)) / MagnetsCnt;
}


void prepareTurnOff() {
  sleepMode = 1;
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("Przygotwywanie...");
  display.display();
  for (servoCurrPos = myservo.read(); servoCurrPos >= 60; servoCurrPos--) {
    myservo.write(servoCurrPos);
    delay(15);
  }

  for (; 1500 <  (millis() - lastMillisSpd);) { // zmiana z 1000 na 1500 w 1.13.19
    digitalWrite(ServoSwitch, LOW);
    digitalWrite(ServoPin, LOW);
    //INFO
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(5, 30);
    display.println("Mozna teraz");
    display.setCursor(5, 38);
    display.println("bezpiecznie wylaczyc");
    display.setCursor(5, 46);
    display.println("komputer.");
    display.setCursor(5, 0);
    display.write(31);
    display.setCursor(10, 0);
    display.println("+");
    display.setCursor(15, 0);
    display.write(30);
    //Version
    display.setCursor(5, 13);
    display.println("V: ");
    display.setCursor(15, 13);
    display.println(Version);   
    //RUN TIME
    run_mins = floor((millis() / 1000) / 60);
    run_hrs = floor(run_mins / 60);
    run_mins = run_mins - (run_hrs * 60);
    display.setCursor(65, 54);
    display.println("T:");
    display.setCursor(80, 54);
    display.println(run_hrs);
    display.setCursor(88, 54);
    display.println(":");
    display.setCursor(93, 54);
    display.println(run_mins);
    adcBattVoltValue = analogRead(VoltInptPin);
    measuredVoltage = (adcBattVoltValue * referenceVoltage) / maxADCValue;
    inputVoltage = measuredVoltage * voltageDividerRatio;  
    voltBarHeight = int(((inputVoltage-9)/3)*64); //odjemowanie 9 bo to minimalne napiecie, podział przez 3 bo zakladam max napiecie 12.0V a nie 12.6V
    voltBarPosition = 64 - voltBarHeight;
    display.fillRect(0, voltBarPosition, 2, voltBarHeight, SSD1306_WHITE);
    display.display();
    delay(500);
  }
}

void calcSpeed(){
  calcSpeed3 = calcSpeed2;
  calcSpeed2 = calcSpeed1;
  calcSpeed1 = raw_speed;

  calcSpeedAvg = (calcSpeed1 + calcSpeed2 + calcSpeed3)/3;
  calcSpeedMain = calcSpeedAvg;

  if(abs(raw_speed - calcSpeed2) >= speedDiffKmh) {
    if ((raw_speed - calcSpeed2) < 0) {
        speedTrend = -1;
    } else {
        speedTrend = 1;
    }
  }else {
    speedTrend = 0;
  }
}
void calcGear() {
  accelerationShift = 1;

  if (calcSpeedMain  >= 0 && calcSpeedMain  < spdRange1and2) {
    calculatedGear = 1;
    currentGearRangeLower = 2.5;
    currentGearRangeUpper = spdRange1and2;
  } else if (calcSpeedMain >= spdRange1and2 && calcSpeedMain < spdRange2and3) {
    calculatedGear = 2;
    currentGearRangeLower = spdRange1and2;
    currentGearRangeUpper = spdRange2and3;
  } else if (calcSpeedMain >= spdRange2and3 && calcSpeedMain < spdRange3and4) {
    calculatedGear = 3;
    currentGearRangeLower = spdRange2and3;
    currentGearRangeUpper = spdRange3and4;
  } else if (calcSpeedMain >= spdRange3and4 && calcSpeedMain < spdRange4and5) {
    calculatedGear = 4;
    currentGearRangeLower = spdRange3and4;
    currentGearRangeUpper = spdRange4and5;
  } else if (calcSpeedMain >= spdRange4and5 && calcSpeedMain < spdRange5and6) {
    calculatedGear = 5;
    currentGearRangeLower = spdRange4and5;
    currentGearRangeUpper = spdRange5and6;
  } else if (calcSpeedMain >= spdRange5and6 && calcSpeedMain < spdRange6and7) {
    calculatedGear = 6;
    currentGearRangeLower = spdRange5and6;
    currentGearRangeUpper = spdRange6and7;
  } else if (calcSpeedMain >= spdRange6and7 && calcSpeedMain < spdRange7and8) {
    calculatedGear = 7;
    currentGearRangeLower = spdRange6and7;
    currentGearRangeUpper = spdRange7and8;
  } else if (calcSpeedMain >= spdRange7and8) {
    calculatedGear = 8;
    currentGearRangeLower = spdRange7and8;
    currentGearRangeUpper = 60.0;
  } else {
    calculatedGear = 8; //Default
  };

  calcTimeDiff = millis() - lastGearCalc;
  if (calcTimeDiff < GearDelayMs && (currentGear - calculatedGear) == 1){
        currentGear = currentGear;
        pointerVisibility = 0;
  } else {
      if (currentGear != calculatedGear) {
        lastGearCalc = millis();
      }
      currentGear = calculatedGear;
      pointerVisibility = 1;
  }
}



//###########################################  LOOP ############################################################
//###########################################  LOOP ############################################################



void loop() {

  loopTime = millis();
  display.clearDisplay();
//  //DIAG Btn1
//  display.setTextSize(1);
//  display.setCursor(0, 30);
//  display.println("Btn1:");
//  display.setCursor(30, 20);
//  display.println(digitalRead(Btn1));
//  display.setCursor(30, 30);
//  display.println(analogRead(Btn1));
//  display.setCursor(55, 30);

  display.setTextSize(3);

  //################################################
  //SPEED
  sleepSpd  =  millis() - millissSpd;
  if (sleepSpd >= 1000.0) {             //podaj zerową prędkość jeśli nie było odcztu od 1,1 s
    raw_speed = 0.0;
    calcSpeed3 = 0.0;
    calcSpeed2 = 0.0;
    calcSpeed1 = 0.0;
  }
    //przejście w tryb uśpienia za przuycisku lub czasu
    if ((digitalRead(Btn2) == HIGH)|| (sleepSpd >= LongTimeToSleepMs)) {
        prepareTurnOff();
    }

  calcSpeed();
  calcGear();
  displGear = 9 - currentGear;
  setPosition(currentGear);


  //duzy font
  //GEAR
  display.setCursor(8, 0);
  display.println("G:");
  display.setCursor(40, 0);
  display.print(currentGear);
  //SPEED_TREND
  display.setCursor(75, 00);
  if (speedTrend <= -1 ) {
    display.write(31);
  } else {
    if (speedTrend >= 1) {
      display.write(30);
    } else {
      display.println("-");
    }
  }
  //SPEED
  display.setCursor(8, 40);
  display.println("S:");
  display.setCursor(40, 40);
  display.println(calcSpeedMain, 1);
  //GearRangePointer
    if (pointerVisibility == 1){
      display.fillRect(115, 32, 7, 1, SSD1306_WHITE);
      display.fillRect(115, 0, 7, 1, SSD1306_WHITE);
      display.fillRect(115, 63, 7, 1, SSD1306_WHITE);
      display.setTextSize(2);
      pointerPosition = 64 - int(((calcSpeedMain - currentGearRangeLower) / (currentGearRangeUpper - currentGearRangeLower)) * 64) - 5;
      display.setCursor(115, pointerPosition);
      display.write(16);
      display.setTextSize(3);
    }

  //VOLT_BAR
  adcBattVoltValue = analogRead(VoltInptPin);
  measuredVoltage = (adcBattVoltValue * referenceVoltage) / maxADCValue;
  inputVoltage = measuredVoltage * voltageDividerRatio;  
  voltBarHeight = int(((inputVoltage-9)/3)*64); //odjemowanie 9 bo to minimalne napiecie, podział przez 3 bo zakladam max napiecie 12.0V a nie 12.6V
  voltBarPosition = 64 - voltBarHeight;
  display.fillRect(0, voltBarPosition, 2, voltBarHeight, SSD1306_WHITE);
  //########################################## ZAPIS DO WYŚWIETLACZA ######################################################
  display.display();
  loopTime = millis();

  if ((digitalRead(Btn2) == HIGH)) {
        digitalWrite(PinLED, HIGH);
      }else {
        digitalWrite(PinLED, LOW);
      }

  //wstrzymanie pętli by odczyty były co 0,5s
  for (; (millis() - loopTime) < 300 ;) {
    delay(10);
  }



  if (speedTrend == -1 ) {
    if (BrakingLightSwitch == 1) {
      digitalWrite(BrakingLight, HIGH);
      BrakingLightSwitch = 0;
    } else {
      digitalWrite(BrakingLight, LOW);
      BrakingLightSwitch = 1;
    }
  } else {
    digitalWrite(BrakingLight, LOW);
  }

}
//###########################################  LOOP ############################################################
//###########################################  LOOP ############################################################