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

#define Version "1.15.8"
////2DO:
//diagnostyka i/lub przeciwdziałanie skokom predkosci
//dlaczego wskaznik odnosi sie do poprawnego biegu a w tym czasie bieg jest zly? bo czas ponizej 2s?
//zmienic system usypiania dodac bezwzglednie czas 3 min, i po czeku z przyciskiem ponowny odczyt magnesu po 0,5sec

// 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 1        //on board: TX
#define ServoSwitch 4       //on board: 4
#define ServoPin 8          //on board: 8
#define BrakingLight 9      //on board: 9    BrakingLight
#define PinLED 10           //on board: 10   ORANGE loop signal
#define Btn1 18             //on board: A0   Button1
#define VoltInptPin 19      //on board: A1   Battery Voltage

#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 TimeToSleep 5000 //5 sec
#define LongTimeToSleep 150000 //150 sec

Servo myservo;


//SPEED
double speed = 0;
double speed_last = 0.0;
double speed_last_2 = 0.0;
double speed_last_3 = 0.0;
int speedTrend = 0;
double sigleTimeSpd = 0.0;
double sleepSpd = 0.0;
unsigned long millissSpd = millis();
unsigned long lastMillisSpd = millis();
unsigned long lastLastMillisSpd = millis();
unsigned long loopTime = millis();
//speed validation
double speed4Gear = 0.0;
double speed4Gear_1 = 0.0;
double speed4Gear_2 = 0.0;
double speed4Gear_3 = 0.0;
double speed4Geat_estimated = 0.0;
double speedDiff_1 = 0.0;
double speedDiff_2 = 0.0;
double speedDiff_3 = 0.0;
int avgWeight_2 = 2;
int avgWeight_3 = 1;
//GEAR
int currentGear = 1;
int previousGear = 1;
int calculatedGear = 1;
//Przedziały dia biegów
float spdRange1and2 = 7.5;
float spdRange2and3 = 11.0;
float spdRange3and4 = 15.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 changeDelayMs = 2000.0;
double accelerationShift = 1.0;
int displGear = 9 - currentGear;
float currentGearRangeLower = 0;
float currentGearRangeMiddle = 3.0;
float currentGearRangeUpper = 7.5;
//SERVO
int pos = 0;
int sleepMode = 0;
int servoCurrPos = ServoMaxAngle;
//GearBar
int gearBarHeight = 0;
int gearBarPosition = 0;
float speedForBar = 0;
//DST
int totalDistMemLocation = 60;
int loop_cnt = 0;
double totalDist = 0.0;
unsigned int totalDistReaded = 0;
unsigned int totalDistWrited = 0;
//BATTERY
float referenceVoltage = 5.1;
int maxADCValue = 1023;
float  voltageDividerRatio = 3.0;
int adcBattVoltValue = 0;
float inputVoltage = 0.0;
float measuredVoltage  = 0.0;
int voltBarHeight = 0;
int voltBarPosition = 0;
//oth
int BrakingLightSwitch;
int ups = 0;
int downs = 0;
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);
//
//  Serial.begin(9600);

//  //DIPLAY settings
//  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
//    Serial.println(F("SSD1306 allocation failed"));
//    for (;;); // Don't proceed, loop forever
//  }
  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(Btn1, INPUT);
  pinMode(VoltInptPin, INPUT);
  //OUTPUT
  pinMode(PinLED, OUTPUT);
  pinMode(BrakingLight, OUTPUT);
  pinMode(ServoSwitch, OUTPUT);

  //Interrupts
  attachInterrupt(digitalPinToInterrupt(PinInSpeed), calcSpeed, 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("TimeToSleep:");
  display.setCursor(75, 30);
  display.println(TimeToSleep);
  display.setCursor(0, 45);
  display.println("ServoMaxAgl:");
  display.setCursor(75, 45);
  display.println(ServoMaxAngle);
  display.display();
  for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
    myservo.write(servoCurrPos);
    delay(15);
  }
  //MEMORY
  eeprom_read_block(&totalDistReaded, totalDistMemLocation, 2);
  totalDist = float(totalDistReaded);
}

//###########################################  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() - lastMillisSpd;
  if (sleepSpd >= 1500) {             //podaj zerową prędkość jeśli nie było odcztu od 1,5 s
    speed4Gear = 0.0;
    speed4Gear_2 = 0.0;
    speed = 0.0;
    speed_last = 0.0;
    speed_last_2 = 0.0;
    speed_last_3 = 0.0;
    //przejście w tryb uśpienia za pomocą odpowiedniego ustawienia koła i magnesu
    if ((digitalRead(PinInSpeed) == LOW)|| (sleepSpd >= LongTimeToSleep)) {
       display.fillCircle(75, 10, 10, SSD1306_WHITE); 
        if (sleepSpd >= TimeToSleep) {
          prepareTurnOff();
        }
      }
    }

  if (speed4Gear > 40 || isinf(speed4Gear)) {
    speed4Gear = speed4Gear_3;
  }
  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(speed, 1);
  //GearBar
  currentGearRangeMiddle = (currentGearRangeLower + currentGearRangeUpper) / 2;
  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);

  gearBarHeight = 64 - int(((speed4Gear - currentGearRangeLower) / (currentGearRangeUpper - currentGearRangeLower)) * 64) - 5;
  display.setCursor(115, gearBarHeight);
  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, 1, voltBarHeight, SSD1306_WHITE);
  //########################################## ZAPIS DO WYŚWIETLACZA ######################################################
  display.display();
  loopTime = millis();

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



  if (speedTrend == -1 && speed4Gear > 0.0 ) {
    if (BrakingLightSwitch == 1) {
      digitalWrite(BrakingLight, HIGH);
      BrakingLightSwitch = 0;
    } else {
      digitalWrite(BrakingLight, LOW);
      BrakingLightSwitch = 1;
    }
  } else {
    digitalWrite(BrakingLight, LOW);
  }
  //TTL DST
//  totalDist = 0;
  loop_cnt = loop_cnt +1;
  if (loop_cnt >= 20 //&& totalDistWrited != totalDist
  ){
    totalDistWrited = int(totalDist);
//    eeprom_write_block(&totalDistWrited,totalDistMemLocation,2);
    loop_cnt = 0;
  } 
}
//###########################################  LOOP ############################################################
//###########################################  LOOP ############################################################
void calcSpeed() {
  lastLastMillisSpd = lastMillisSpd;
  lastMillisSpd = millissSpd;
  millissSpd = millis();
  sigleTimeSpd = double(millissSpd - lastLastMillisSpd) / 1000;
//  speed_last_3 = speed_last_2;
//  speed_last_2 = speed_last;
//  speed_last = speed;
  speed  = (((2 * Pi) / sigleTimeSpd * ((WheelCircumference) / (Pi)) * ms2kmh)) / MagnetsCnt;
  //  diag
//    if (((speed_last_3 * 1.5) > speed) && speed > 10.0 && speedTrend  > 0) {   //zabezpieczenie przed losowymi sygnałami magesu
//      speed = speed_last_3;
//    }
//    speed = (speed + speed_last)/2;
    totalDist = totalDist + (1/MagnetsCnt);
}

void calcGear() {
  //speed validation
  speedDiff_3 = speed4Gear_3-speed4Gear_2;
  speedDiff_2 = speed4Gear_2-speed4Gear_1;
  speed4Gear_3 = speed4Gear_2;
  speed4Gear_2 = speed4Gear_1;
  speed4Gear_1 = speed;
  speed4Geat_estimated = (((speedDiff_3 * avgWeight_3 + speedDiff_2 * avgWeight_2)/(avgWeight_3 + avgWeight_2))+ speed4Gear_1) + 4;
  if (speed <= 10.0 or speed <= speed4Geat_estimated) {
     speed4Gear_2 = speed4Gear;
    speed4Gear = speed4Gear_1;
  }


//poniżej zamienić speed  na speed4Gear
//speed_last_3 na speed4Gear_2



  
  if ((speed4Gear / speed4Gear_2) >= 1.5) { // przyspieszenie DO weryfikacji czy nie trzeba zamienic na czas lub zwiększyc wartość
    speedTrend = 1;
  } else if ((speed4Gear - speed4Gear_2) <= -1.5) {
    speedTrend = -1;
  } else {
    speedTrend = 0;
  }

  //  if (speedTrend > 0){        // wymusza wczerśniejszą zmianę biegów gdy wykryto przyspieszanie
  //    accelerationShift = 1.05;
  //  }else {
  accelerationShift = 1;
  //  }
  if (speed4Gear * accelerationShift >= 0 && speed4Gear * accelerationShift < spdRange1and2) {
    calculatedGear = 1;
    currentGearRangeLower = 2.5;
    currentGearRangeUpper = spdRange1and2;
  } else if (speed4Gear * accelerationShift >= spdRange1and2 && speed4Gear * accelerationShift < spdRange2and3) {
    calculatedGear = 2;
    currentGearRangeLower = spdRange1and2;
    currentGearRangeUpper = spdRange2and3;
  } else if (speed4Gear * accelerationShift >= spdRange2and3 && speed4Gear * accelerationShift < spdRange3and4) {
    calculatedGear = 3;
    currentGearRangeLower = spdRange2and3;
    currentGearRangeUpper = spdRange3and4;
  } else if (speed4Gear * accelerationShift >= spdRange3and4 && speed4Gear * accelerationShift < spdRange4and5) {
    calculatedGear = 4;
    currentGearRangeLower = spdRange3and4;
    currentGearRangeUpper = spdRange4and5;
  } else if (speed4Gear * accelerationShift >= spdRange4and5 && speed4Gear * accelerationShift < spdRange5and6) {
    calculatedGear = 5;
    currentGearRangeLower = spdRange4and5;
    currentGearRangeUpper = spdRange5and6;
  } else if (speed4Gear * accelerationShift >= spdRange5and6 && speed4Gear * accelerationShift < spdRange6and7) {
    calculatedGear = 6;
    currentGearRangeLower = spdRange5and6;
    currentGearRangeUpper = spdRange6and7;
  } else if (speed4Gear * accelerationShift >= spdRange6and7 && speed4Gear * accelerationShift < spdRange7and8) {
    calculatedGear = 7;
    currentGearRangeLower = spdRange6and7;
    currentGearRangeUpper = spdRange7and8;
  } else if (speed4Gear * accelerationShift >= spdRange7and8) {
    calculatedGear = 8;
    currentGearRangeLower = spdRange7and8;
    currentGearRangeUpper = 60.0;
  } else {
    calculatedGear = 8; //Default
  };
  calcTimeDiff = millis() - lastGearCalc;

  if (calculatedGear == currentGear) {
    speedForBar = speed4Gear * accelerationShift;
  }
  if ((calculatedGear + 1) < currentGear || (calculatedGear - 1) > currentGear || calcTimeDiff >= changeDelayMs || speedTrend > 0 ) {
    //zmień bieg tylko, gdy rożnica między biegiem wyliczonym a obecnym jest większa niż jeden lub gdy od zmieny biegu minely 3 sec
    if (currentGear > calculatedGear) {
      downs = downs + 1;
    }
    if (currentGear < calculatedGear) {
      ups = ups + 1;
    }
    currentGear = calculatedGear;
    previousGear = currentGear;
    lastGearCalc = millis();
    speedForBar = speed4Gear * accelerationShift;

  }
  if  (speedTrend >= 1 and calculatedGear < 8) {
    if (currentGear > calculatedGear) {
      downs = downs + 1;
    }
    if (currentGear < calculatedGear) {
      ups = ups + 1;
    }
    currentGear = calculatedGear;
    previousGear = currentGear;
    lastGearCalc = millis();
    speedForBar = speed4Gear * accelerationShift;

  }  else if (speedTrend <= -1 and calculatedGear > 1) {
    if (currentGear > calculatedGear) {
      downs = downs + 1;
    }
    if (currentGear < calculatedGear) {
      ups = ups + 1;
    }
    previousGear = currentGear;
    currentGear = calculatedGear;
    lastGearCalc = millis();
    speedForBar = speed4Gear * accelerationShift;
  };
}

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 && speed > 0.0) {
    digitalWrite(ServoSwitch, HIGH);
    for (servoCurrPos = myservo.read(); servoCurrPos <= 180; servoCurrPos++) {
      myservo.write(servoCurrPos);
      delay(4);
    }
    sleepMode = 0;
  }
  myservo.write(pos);
}

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);
  }
//Na czas diagnostyki
//    eeprom_write_block(&totalDist,totalDistMemLocation,2);
//    totalDistWrited = totalDist;
//    loop_cnt = 0;
//    
  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(0, 30);
    display.println("Mozna teraz");
    display.setCursor(0, 38);
    display.println("bezpiecznie wylaczyc");
    display.setCursor(0, 46);
    display.println("komputer.");
    display.setCursor(0, 0);
    display.write(31);
    display.setCursor(5, 0);
    display.println("+");
    display.setCursor(10, 0);
    display.write(30);
    display.setCursor(20, 0);
    display.println(downs + ups);
    //Version
    display.setCursor(0, 13);
    display.println("V: ");
    display.setCursor(10, 13);
    display.println(Version);   
    //DST
    display.setCursor(45, 0);
    display.println("Spins: ");
    display.setCursor(80, 0);
    display.println(totalDistWrited);
    display.setCursor(53, 13);
    display.println("KM: ");
    display.setCursor(90, 13);
    display.println((totalDistWrited*WheelCircumference)/1000);
    //RUN TIME
    run_mins = floor((millis() / 1000) / 60);
    run_hrs = floor(run_mins / 60);
    run_mins = run_mins - (run_hrs * 60);
    display.setCursor(60, 54);
    display.println("T:");
    display.setCursor(75, 54);
    display.println(run_hrs);
    display.setCursor(83, 54);
    display.println(":");
    display.setCursor(88, 54);
    display.println(run_mins);
    display.display();
    delay(500);
  }
}