#include <MultiMap.h>
#include "Wire.h"
#include "Adafruit_GFX.h"
#include <Adafruit_SSD1305.h>
#include <MonomaniacOne_Regular36pt7b.h>
#include <Adafruit_NeoPixel.h>
#include "driver/twai.h"

#define TWAI_TIMING_CONFIG_33k3KBITS()   {.brp = 120, .tseg_1 = 15, .tseg_2 = 4, .sjw = 3, .triple_sampling = false}  //SW-SL-CAN obd 1 - 5


#define LED_PIN       10
Adafruit_NeoPixel pixels(1, LED_PIN, NEO_GRB + NEO_KHZ800);

Adafruit_SSD1305 display(128, 64, &Wire, -1);


#define GREEN_LED     11
#define RX_PIN        13
#define TX_PIN        14
#define CAN_RS        38


#define FORCE_ON      47
#define SENSE_V_DIG   12
#define SENSE_V_ANA   9

#define SECONDS_TO_STAY_ON   15

#define CANPID_RPM                  0x0C          //OBD2
#define CANPID_SPEED                0x0D          //OBD2
#define CANPID_TEMP                 0x05          //OBD2
#define CAN_REQST_ID                0x7DF         //OBD2
#define CAN_REPLY_ID                0x7E8         //OBD2

#define ID_RPM_SPEED_OPEL_CORSA     0x108         //OPEL CORSA
#define ID_COOLANT_OPEL_CORSA       0x145         //OPEL CORSA
//#define ID_..._OPEL_CORSA         0x7DF         //OPEL CORSA
//#define ID_..._OPEL_CORSA         0x7E8         //OPEL CORSA


int speed_string_length; // length of the speed_string
int speed_string_start_pos; // start x position for the big numbers - calculated based on the number of digits
char speed_string[10];
int temp_string_length; // length of the speed_string
int temp_string_start_pos; // start x position for the big numbers - calculated based on the number of digits
char temp_string[10];
int RPM_string_length; // length of the speed_string
int RPM_string_start_pos; // start x position for the big numbers - calculated based on the number of digits
char RPM_string[10];
int cruise_string_length; // length of the speed_string
int cruise_string_start_pos; // start x position for the big numbers - calculated based on the number of digits
char cruise_string[10];


  //  out[] holds the voltages
float out[] = {51, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160};

  //  in[] holds the measured analogRead() values for that voltage
float in[]  = {1080, 1323, 1575, 1820, 2075, 2318, 2565, 2809, 3065, 3340, 3665, 4050};



int speed = 0;
int temp = 0;
int RPM = 0;
int cruise = 0;
int voltage = 0;


void setup() {
  Serial.begin(115200);     // INITIALIZE Serial
  pixels.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
  pixels.show();            // Turn OFF all pixels ASAP
  pixels.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)

  Serial.setTxTimeoutMs(0);   // prevent serial print delays if no usb

  pinMode(SENSE_V_DIG, INPUT_PULLUP);
  pinMode(FORCE_ON, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(CAN_RS, OUTPUT);    // INPUT (high impedance) = slope control mode, OUTPUT = see next line
  digitalWrite(CAN_RS, LOW);  // LOW = high speed mode, HIGH = low power mode (listen only)
  Serial.println("RS LOW");
  twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)TX_PIN, (gpio_num_t)RX_PIN, TWAI_MODE_NORMAL);  // TWAI_MODE_NORMAL, TWAI_MODE_NO_ACK or TWAI_MODE_LISTEN_ONLY
  twai_timing_config_t t_config  = TWAI_TIMING_CONFIG_33k3KBITS();
  twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();


    // Install TWAI driver
  if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) {
    Serial.println("Driver installed");
  } else {
    Serial.println("Failed to install driver");
    return;
  }

  // Start TWAI driver
  if (twai_start() == ESP_OK) {
    Serial.println("Driver started");
  } else {
    Serial.println("Failed to start driver");
    return;
  }

  digitalWrite(FORCE_ON, HIGH);

  delay(2000);
  Wire.setPins(1,2);        // INITIALIZE I2C
  delay(1000);
  Serial.println("SSD1305 OLED test");

  if ( ! display.begin(0x3C) ) {
     Serial.println("Unable to initialize OLED");
     while (1) yield();
  }
  display.setTextColor(WHITE);
  update(0, 0, 0, 0); // Initial speed

}


void loop() {

    shutdowncounter();
    digitalWrite(GREEN_LED, analogRead(SENSE_V_ANA)>3200);

    //log_CAN_messages();
    can_receiveOPEL();
    //can_sendrequestOBD2(CANPID_RPM);
    //can_receiveOBD2();
    //can_sendrequestOBD2(CANPID_SPEED);
    //can_receiveOBD2();
    //can_sendrequestOBD2(CANPID_TEMP);
    //can_receiveOBD2();

    cruise = multiMap<float>(analogRead(SENSE_V_ANA), in, out, 12);

    update(speed, temp, RPM, cruise);

    delay(10);

}

void update(int speed, int temp, int RPM, int cruise) {
  display.setRotation(0);
  display.clearDisplay();

  display.setFont(&MonomaniacOne_Regular36pt7b);
  display.setTextSize(1);
  itoa (speed, speed_string, 10); // convert speed integer to c-style string speed_string, decimal format
  speed_string_length = strlen(speed_string); // get speed_string length
  speed_string_start_pos = 20 - speed_string_length * 19; // start x position of the big numbers
  display.setCursor(speed_string_start_pos + 44, 48);                       // Adjust position
  display.print(speed);

  display.setFont();
  display.setTextSize(1);
  itoa (temp, temp_string, 10); // convert speed integer to c-style string speed_string, decimal format
  temp_string_length = strlen(temp_string); // get speed_string length
  temp_string_start_pos = 6 - temp_string_length * 3; // start x position of the big numbers
  display.setCursor(temp_string_start_pos + 5, 55);
  display.print(temp);

  display.setFont();
  display.setTextSize(1);
  itoa (RPM, RPM_string, 10); // convert speed integer to c-style string speed_string, decimal format
  RPM_string_length = strlen(RPM_string); // get speed_string length
  RPM_string_start_pos = 6 - RPM_string_length * 3; // start x position of the big numbers
  display.setCursor(RPM_string_start_pos + 60, 55);
  display.print(RPM);

  display.setFont();
  display.setTextSize(1);
  itoa (cruise, cruise_string, 10); // convert speed integer to c-style string speed_string, decimal format
  cruise_string_length = strlen(cruise_string); // get speed_string length
  cruise_string_start_pos = 6 - cruise_string_length * 3; // start x position of the big numbers
  display.setCursor(cruise_string_start_pos + 110, 55);
  display.print(cruise);



  display.display();
}

void shutdowncounter(void) {
  static unsigned long timestamp;
  static bool countdown = false;
  unsigned long now = millis();

  if( analogRead(SENSE_V_ANA)<3200 ) {
    if ( countdown == false ) {
      countdown = true;
      timestamp = now;
      pixels.setPixelColor(0, pixels.Color(150, 0, 0));
      pixels.show();
     }
  }
  else {
    countdown = false;
    pixels.setPixelColor(0, pixels.Color(0, 0, 150));
    pixels.show();
  }
  if ( countdown  &&  now - timestamp >= SECONDS_TO_STAY_ON * 1000 ) {
    digitalWrite(FORCE_ON, LOW);
    pixels.setPixelColor(0, pixels.Color(150, 0, 150));
    pixels.show();
  }
}

void log_CAN_messages() {  //savvycan format
  twai_message_t message;
  if (twai_receive(&message, pdMS_TO_TICKS(1)) == ESP_OK) {
    Serial.print("("); Serial.print(millis()/1000); Serial.print(") ");
    Serial.print("vcan0 ");
    Serial.print(message.identifier, HEX);
    Serial.print("#");
    for (int i = 0; i < message.data_length_code; i++) {
      Serial.printf("%02X", message.data[i]);
    }
    Serial.println();
  }
}


void can_sendrequestOBD2(uint8_t obd2) {
  twai_message_t message;
  message.identifier = CAN_REQST_ID;
  message.extd = 0;
  message.rtr = 0;
  message.data_length_code = 8;
  message.data[0] = 2;
  message.data[1] = 1;
  message.data[2] = obd2;
  message.data[3] = 0xAA;
  message.data[4] = 0xAA;
  message.data[5] = 0xAA;
  message.data[6] = 0xAA;
  message.data[7] = 0xAA;
  twai_transmit(&message, pdMS_TO_TICKS(1));
}


void can_receiveOBD2(void) {
  twai_message_t message;
  if (twai_receive(&message, pdMS_TO_TICKS(1)) == ESP_OK) {  // Wait up to 10ms for a message
    if (message.identifier == CAN_REPLY_ID) { // Make sure it's the right reply
      if (message.data[2] == CANPID_RPM) {
        uint8_t rpmOBDH = message.data[3];
        uint8_t rpmOBDL = message.data[4];
        RPM = (uint16_t) ((256 * rpmOBDH) + rpmOBDL) / 4;
      } else if (message.data[2] == CANPID_SPEED) {
        speed = (uint16_t) message.data[3];  // Speed is a single byte
      } else if (message.data[2] == CANPID_TEMP) {
        temp = (uint16_t) message.data[3] - 40;  // Adjust for OBD-II temperature offset
      }
    }
  }
}

void can_receiveOPEL(void) {
  twai_message_t message;
  if (twai_receive(&message, pdMS_TO_TICKS(25)) == ESP_OK) {  // Wait up to 10ms for a message
    if (message.identifier == ID_RPM_SPEED_OPEL_CORSA) { // check id RPM/SPEED
      uint8_t firstRPM = message.data[1];
      uint8_t secndRPM = message.data[2];
      RPM = (uint16_t) ((256*firstRPM) + secndRPM) / 4;
      uint8_t firstSPEED = message.data[4];
      uint8_t secndSPEED = message.data[5];
      speed = (uint16_t) ((256*firstSPEED) + secndSPEED) * 0.0082;

    } else if (message.identifier == ID_COOLANT_OPEL_CORSA) { // check id COOLANT temperature
      temp = (uint8_t) message.data[3] - 40;  // -40 is adjustment --> -40degrees to 215
    }
  }
}