#include "driver/twai.h"


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


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


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


void setup() {
  Serial.begin(115200);

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

  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_500KBITS();
  twai_filter_config_t f_config = {
    .acceptance_code = (CAN_REPLY_ID << 21),
    .acceptance_mask = ~(0x7E8 << 21), // Only accept 0x7E8
    .single_filter = true
    };
    // 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;
  }

}


void loop() {

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

    update(speed, temp, RPM);

    delay(200);

}

void update(int speed, int temp, int RPM) {

  Serial.print("RPM");
  Serial.println(RPM);
  Serial.print("temperature");
  Serial.println(temp);
  Serial.print("speed");
  Serial.println(speed);



}


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(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
      }
    }
  }
}