#include <Arduino.h>
#include <Preferences.h>
#include <EEPROM.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSerifItalic24pt7b.h>
#include <CAN.h> // the OBD2 library depends on the CAN library
// Copyright (c) Sandeep Mistry. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

#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     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);
const int white = SSD1306_WHITE;

#define button 12
#define ENCODER_CLK 27
#define ENCODER_DT  14
int page = 0;
int oldPage = 0;

int soot = 0;
float diffPressureVolt = 0;
int diffPressureKPa = 0;
int regens = 0;
int replacemaentdist = 0;
int lastregendist = 0;
int regenstate = 0;

void changePage(){
  if (page >= 6){page = 0;}
  else{page = page + 1;}
  Serial.println(page);
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue == HIGH) {
    Serial.println("Rotated clockwise ⏩");
    if(page >=5){page = 0;}
    else{page=page+1;}
    Serial.println(page);
  }
  if (dtValue == LOW) {
    Serial.println("Rotated counterclockwise ⏪");
    if (page <= 0){page = 5;}
    else{page=page-1;}
    Serial.println(page);
  }
}

void setup() {
  Wire.begin(5,4);
  Serial.begin(9600);
  while (!Serial);
  Serial.println(F("OBD2 Key Stats"));

  EEPROM.begin(12);
  page = EEPROM.read(1);
  Serial.println(page);
  pinMode(button, INPUT_PULLUP);
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);
  attachInterrupt(digitalPinToInterrupt(button), changePage, RISING);

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }    
  display.clearDisplay();                      // Clear the buffer
  display.setTextSize(2);                     // Normal 2:1 pixel scale
  display.setTextColor(white);         // Draw white text
  display.setCursor(5,0);                    // Start at top-left corner
  display.print("CORSA DPF");
  display.drawFastHLine(0,17,127,white);
  display.display();

  

  while (true) {
    Serial.print(F("Attempting to connect to OBD2 CAN bus ... "));

    if (!CAN.begin(500E3)) {
      Serial.println(F("failed!"));
      //display.setTextSize(1);
      //display.setTextColor(SSD1306_WHITE);
      //display.setCursor(0,42);
      //display.print(F("CAN BUS Init Fail!"));
      //display.display();
      //delay(1000);
    } else {
      CAN.filter(0x5E8);
      Serial.println(F("success"));
      //display.setTextSize(1);
      //display.setTextColor(SSD1306_WHITE);
      //display.setCursor(0,42);
      //display.print(F("CAN BUS Init OK!"));
      //display.display();      
      break;
    }
  }

  Serial.println();
  
}

unsigned char _data29[0x8] = {0x29, 0x68, 0x13, 0x16, 0x00, 0x0d, 0x00, 0x00};
unsigned char _data2a[0x8] = {0x2a, 0x00, 0x1a, 0xc2, 0x00, 0x03, 0x96, 0x00};

int readAA(unsigned char pid, unsigned char * resData, unsigned char resLen) {

  for (int retries = 10; retries > 0; retries--) {
    CAN.beginPacket(0x7E0);
    CAN.write(0x03); //length
    CAN.write(0xAA);
    CAN.write(0x01); //single shot
    CAN.write(pid);
    if (CAN.endPacket()) {
      // send success
      break;
    } else if (retries <= 1) {
      return 0;
    }
  }


  int packetSize=0;
  unsigned long start = millis();
  int _responseTimeout = 2000; //timeout to receive
  int _index=0;

  while(packetSize == 0 && (millis() - start) < _responseTimeout) {
    packetSize = CAN.parsePacket();
    if(packetSize > 0) {
      //Serial.println(packetSize);
      //Serial.print(CAN.packetId(),HEX);
      //Serial.print(", ");

      while (CAN.available() && _index<packetSize) {
        unsigned char b = CAN.read();
        resData[_index]=b;
        _index++;
        //Serial.print(b,HEX);
        //Serial.print(" ");
      }
      //Serial.println();
    }
  }
  return packetSize;
}


void calculate29(){
  soot = (_data29[1]*200)/255;
  diffPressureVolt = (_data29[2]*5.0)/255;
  diffPressureKPa = (_data29[3])-6;
  regens = (_data29[4]*256)+ _data29[5];
}


void calculate2a(){
  replacemaentdist = (_data2a[1]*65536)+(_data2a[2]*256)+_data2a[3];  //3 bytes A*65536+B*256+C min=0, max = 100000 km.
  lastregendist = (_data2a[4]*65536)+(_data2a[5]*256)+_data2a[6]; //3 bytes D*65536+E*256+F min=0, max = 100000 km.
  regenstate = (_data2a[7]*100)/255;  // Status of regeneration 0 - 100% 0 - FF
  //Serial.println(a);
  //Serial.println(regenstate);
  //Serial.println(lastregendist, DEC);
  //Serial.println(replacemaentdist, DEC);
}

void showData(){    //page 0
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  //check Regen status if is in Action INVERT LCD
  if(regenstate>0){
    display.invertDisplay(true);
    page = 6;}
  else{display.invertDisplay(false);}
  
  //Soot 1st line text + 1 horisontal line 
  display.setCursor(2,2);
  display.print(F("Soot: "));
  display.print(soot);
  display.print(F("%"));
  display.drawFastHLine(0,23,127,white);
  
  // last regen distance 2nd line
  display.setCursor(2,30);
  display.print(F("Last:"));
  display.print(lastregendist);
  display.print(F("km"));
  
  // Total distance from exchange 3th line smallest font
  display.setCursor(27,55);
  display.setTextSize(1);
  display.print(F("Total:"));
  display.print(replacemaentdist);
  display.print(F("km"));
  
  //show page on LCD
  display.display();
}

void showSoot(){		//page 1
	display.clearDisplay();
	display.setTextSize(2);
	display.setTextColor(SSD1306_WHITE);
	display.drawFastHLine(0,19,127,white);
	display.setCursor(34,2);
	display.print(F("Soot"));
	if(regenstate>0){
    display.invertDisplay(true);
    page = 6;
		}
	else{display.invertDisplay(false);}
	display.setTextSize(5);
	if (soot >=0 && soot <10){
    display.setCursor(36, 26);
		}
	if (soot >=10 && soot < 100){
    display.setCursor(25,26);
		}
	if (soot >= 100){
    display.setCursor(3,26);
		}
	display.print(soot);
	display.print(F("%"));
	display.display();
}

void showDiffPressureVolt(){		//page 2
	display.clearDisplay();
	display.setTextSize(2);
	display.setTextColor(SSD1306_WHITE);
	display.drawFastHLine(0,19,127,white);
	display.setCursor(20,2);
	display.print(F("Pressure"));
	if(regenstate>0){
    display.invertDisplay(true);
    page = 6;
		}
	else{display.invertDisplay(false);}
	display.setTextSize(5);
	display.setCursor(0, 26);
	display.print(diffPressureVolt);
  display.setTextSize(1);
	display.print(F("V"));
	display.display();
}

void showDiffPressureKPa(){		//page 3
	display.clearDisplay();
	display.setTextSize(2);
	display.setTextColor(SSD1306_WHITE);
	display.drawFastHLine(0,19,127,white);
	display.setCursor(20,2);
	display.print(F("Pressure"));
	if(regenstate>0){
    display.invertDisplay(true);
    page = 6;
		}
	else{display.invertDisplay(false);}
	display.setTextSize(5);
	display.setCursor(36, 26);
	display.print(diffPressureKPa);
  display.setTextSize(1);
	display.print(F("kPa"));
	display.display();
}

void showNumberOfRegens(){		//page 4
	display.clearDisplay();
	display.setTextSize(2);
	display.setTextColor(SSD1306_WHITE);
	display.drawFastHLine(0,19,127,white);
	display.setCursor(30,2);
	display.print(F("Regens"));
	if(regenstate>0){
    display.invertDisplay(true);
    page = 6;
		}
	else{display.invertDisplay(false);}
	display.setTextSize(5);
	if (regens >=0 && regens <10){
    display.setCursor(36, 26);
		}
	if (regens >=10 && regens < 100){
    display.setCursor(25,26);
		}
	if (regens >= 100){
    display.setCursor(3,26);
		}
	display.print(regens);
	//display.print(F("%"));
	display.display();
}

void showKm(){    //page 5
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  //check Regen status
  if(regenstate>0){
    display.invertDisplay(true);
    page = 5;
    }
  else{
    display.invertDisplay(false);
    //page = 0;
    }
  
  //Soot 1st line text + 1 horisontal line 
  display.setCursor(2,2);
  display.print(F("Last:"));
  display.print(lastregendist);
  display.setTextSize(1),
  display.print(F("km"));
  //display.drawFastHLine(0,19,127,white);
  
  // last regen distance 2nd line
  //display.setCursor(2,22);
  //display.print(F("Last:"));
  //display.print(lastregendist);
  //display.print(F("km"));
  
  // last regen distance 3th line
  display.setCursor(2,30);
  display.setTextSize(2);
  display.print(F("Tot:"));
  display.print(replacemaentdist);
  display.setTextSize(1);
  display.print(F("km"));
  
  //show page on LCD
  display.display();
}

void showRegenstate(){  //page 6
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.drawFastHLine(0,19,127,white);
  display.setCursor(34,2);
  display.print(F("Regen"));
  if(regenstate>50){
    display.invertDisplay(true);
    //page = 5;
    }
  else{
    display.invertDisplay(false);
    page = 0;
    }
  display.setTextSize(5);
  if (regenstate >=0 && regenstate <10){
    display.setCursor(36, 26);
    }
  if (regenstate >=10 && regenstate < 100){
    display.setCursor(25,26);
    }
  if (regenstate >= 100){
    display.setCursor(3,26);
    }
  display.print(regenstate);
  display.print(F("%"));
  display.display();

}



void loop() {
  int buff;
  /*
  if(readAA(0x29,_data29,8)) {
    Serial.println("read 29");
    Serial.print(CAN.packetId(),HEX);
    Serial.print(", ");
    for(int i=0; i<8; i++) {
      Serial.print(_data29[i],HEX);
      Serial.print(" ");
    }
    Serial.println();
    calculate29();
  }

  if(readAA(0x2A,_data2a,8)) {
    Serial.println("read 2A");
    Serial.print(CAN.packetId(),HEX);
    Serial.print(", ");
    for(int i=0; i<8; i++) {
      Serial.print(_data2a[i],HEX);
      Serial.print(" ");
    } */

    if (Serial.available() > 0) {
    // read the incoming byte:
      char r = Serial.read();
      if ( r != '\n')
        {
          //Do Something
          regenstate = r;
        }
      Serial.println(regenstate);
    } 
    calculate2a();
    calculate29();
    switch(page){
      case 0:
        EEPROM.write(1, page);
        showData();
        break;
      case 1:
        EEPROM.write(1, page);
        buff = EEPROM.read(1);
        Serial.println(buff);
        showSoot();
        break;
      case 2:
        EEPROM.write(1, page);
        showDiffPressureVolt();
        break;
      case 3:
        EEPROM.write(1, page);
        showDiffPressureKPa();
        break;
      case 4:
        EEPROM.write(1, page);
        showNumberOfRegens();
        break;
      case 5:
        EEPROM.write(1, page);
        showKm();
        break;
      case 6:
        EEPROM.write(1, page);
        showRegenstate();
        break;
      default:
      EEPROM.write(1, page);
      showData();
      break;
    }
  delay(500);  //20 sec between measurements
  
}
/*
void printPID(int pid) {
  // print PID name
  Serial.print(OBD2.pidName(pid));
  Serial.print(F(" = "));
  
  //display.fillRect(x0, y0, width, height, BLACK);
  display.fillRect(0, 18, 127, 45, BLACK);
  display.setFont(&FreeSerifItalic24pt7b);
  display.setTextColor(WHITE); 
  display.setCursor(0,55);
  
  // read the PID value
  float pidValue = OBD2.pidRead(pid);


  if (isnan(pidValue)) {
    Serial.print("error");
    display.print("Err.");
    display.display();
  } else {
    // print value with units
    Serial.print(pidValue);
    Serial.print(F(" "));
    Serial.print(OBD2.pidUnits(pid));
    display.print(pidValue);
    display.display();
  }

  Serial.println();
}*/