#include <Adafruit_NeoPixel.h>
#include "driver/twai.h"


#define LED_PIN       10          //Neopixel led Nasimovy's-board: 10  macchina-A0: 2
#define LED_EN        -1          //Neopixel led ENABLE Nasimovy's-board: -1  macchina-A0: 13
Adafruit_NeoPixel pixels(1, LED_PIN, NEO_GRB + NEO_KHZ800);

#define GREEN_LED     11          //Nasimovy's-board: 11  macchina-A0: -1
#define RX_PIN        13          //Nasimovy's-board: 13  macchina-A0: 4
#define TX_PIN        14          //Nasimovy's-board: 14  macchina-A0: 5
#define CAN_RS        38          //Nasimovy's-board: 38  macchina-A0: 21


#define FORCE_ON      47          //Nasimovy's-board: 47  macchina-A0: -1
#define SENSE_V_ANA   9           //Nasimovy's-board: 9  macchina-A0: 35

#define SECONDS_TO_STAY_ON   60   //unused for macchina-A0

#define CANPID_RPM    0x0C
#define CANPID_SPEED  0x0D
#define CANPID_TEMP   0x05
#define CAN_REQST_ID  0x7DF
#define CAN_REPLY_ID1  0x7E8
#define CAN_REPLY_ID2  0x7E9
#define CAN_REPLY_ID3  0x7EA
#define CAN_REPLY_ID4  0x7EB


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


void setup() {
  Serial.begin(115200);
  Serial.setTxTimeoutMs(0);     // prevent serial print delays if no usb
  pinMode(FORCE_ON, OUTPUT);    // OUTPUT = see next line
  digitalWrite(FORCE_ON, HIGH); // HIGH = keeps boards power sypply on
  pinMode(GREEN_LED, OUTPUT);   // OUTPUT
  pinMode(LED_EN, OUTPUT);      // OUTPUT = see next line
  digitalWrite(LED_EN, HIGH);   // HIGH = neopixel enabled?
  pinMode(CAN_RS, OUTPUT);      // OUTPUT = see next line
  digitalWrite(CAN_RS, LOW);    // LOW = high speed mode, HIGH = low power mode (listen only)
  Serial.println("RS LOW");
  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)

  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_500KBITS();
  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;
  }
  update(0, 0, 0, 0); // Initial values

}


void loop() {

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

    can_sendrequest(CANPID_RPM);
    can_receive();
    can_sendrequest(CANPID_SPEED);
    can_receive();
    can_sendrequest(CANPID_TEMP);
    can_receive();

    voltage = analogRead(SENSE_V_ANA);

    update(speed, temp, RPM, voltage);

    delay(20);

}

void update(int speed, int temp, int RPM, int voltage) {
  Serial.print("speed: ");
  Serial.println(speed);
  Serial.print("temperature: ");
  Serial.println(temp);
  Serial.print("RPM: ");
  Serial.println(RPM);
  Serial.print("voltage: ");
  Serial.println(voltage);

}

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 can_sendrequest (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_receive(void) {
  twai_message_t message;
  if (twai_receive(&message, pdMS_TO_TICKS(10)) == ESP_OK) {  // Wait up to 10ms for a message
    if (message.identifier == CAN_REPLY_ID1||message.identifier == CAN_REPLY_ID2
        ||message.identifier == CAN_REPLY_ID3||message.identifier == CAN_REPLY_ID4) { // 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
      }
    }
  }
}