//#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.3"
//ostatnia zmiana: zmiana na PCB, dostrajanie
////2DO:

#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 PinInSpeed 0        //on board: RXI
#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   Button0
#define Btn2 19             //on board: A1   Button1

#define ServoMaxAngle 130
#define MaxAngle 179
#define MinAngle 1
#define MaxGear 8
#define MinGear 1
#define WheelCircumference  2.130
#define MagnetsCnt 4
#define ms2kmh 3.6
#define Pi 3.1416
#define TimeToSleep 180000 //3 min

Servo myservo;


//SPEED
double speed = 0;
double speed_last = 0;
double speed_last_2 = 0;
double speed_last_3 = 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();
//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 = 23.5;
float spdRange6and7 = 29.3;
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;
//oth
int BrakingLightSwitch;
int ups = 0;
int downs = 0;
int run_hrs = 0;
int run_mins = 0;
//DST
int totalDist = 0;
int totalDistReaded = 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(Btn1, INPUT);
  //OUTPUT
  pinMode(PinLED, OUTPUT);
  pinMode(BrakingLight, OUTPUT);
  pinMode(ServoSwitch, OUTPUT);

  //Interrupts
  attachInterrupt(digitalPinToInterrupt(PinInSpeed), diag, RISING);

  // powolne ustawianie pozycji servo po włączeniu
  //  delay(750);
  //  setPosition(8);

  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();
  //MEMORY
  eeprom_read_block(&totalDistReaded, 0, 2);
  totalDist = totalDistReaded;

}


void loop() {

  display.clearDisplay();
  display.setTextSize(3);
speed = 17.7;//speed +1.0;
  //################################################
  


  //duzy font

  //SPEED_TREND
  display.setCursor(100, 00);
  if (speedTrend <= -1 ) {
    display.write(31);
  } else {
    if (speedTrend >= 1) {
      display.write(30);
    } else {
      display.println("-");
    }
  }

  
//  digitalWrite(PinLED, HIGH);
//  delay(500);
//  digitalWrite(PinLED, LOW);
//  delay(500);

  
  display.display();

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

  ////przejście w tryb uśpienia za pomocą przycisku
  if (digitalRead(Btn1) == HIGH) {
    prepareTurnOff();
  }

}

void diag(){
  digitalWrite(PinLED, HIGH);
  delay(200);
  digitalWrite(PinLED, LOW);
    delay(200);
      digitalWrite(PinLED, HIGH);

}

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(30);
  }
  for (; 1000 <  (millis() - lastMillisSpd);) {
    digitalWrite(ServoSwitch, LOW);
    digitalWrite(ServoPin, LOW);
    //      myservo.detach(ServoPin);
    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);

    display.println("Spins: ");
    display.setCursor(60, 0);
    display.println(totalDist);;
    display.setCursor(70, 0);

    run_mins = floor((millis() / 1000) / 60);
    run_hrs = floor(run_mins / 60);
    run_mins = run_mins - (run_hrs * 60);
    display.setCursor(64, 54);
    display.println("T:");
    display.setCursor(75, 54);
    display.println(run_hrs);
    display.setCursor(80, 54);
    display.println(":");
    display.setCursor(85, 54);
    display.println(run_mins);
    display.display();
    delay(500);
  }
}