#include <SPI.h>
#include <mcp_can.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <SoftwareSerial.h>

SoftwareSerial lora(8, 9); // RX, TX
LiquidCrystal_I2C lcd(0x27, 20, 4);

#define CAN_CS 10
MCP_CAN CAN(CAN_CS);

const byte NODE_ID = 0x01;   // Change to your Sevcon node ID
const unsigned long timing = 500;
const unsigned long timing_c = 10;

unsigned long previous_time = 0;
unsigned long current_time = 0;
unsigned long previous_time_c = 0;

int16_t rpm = 0;
int16_t ubat = 0;
int v = 0;
int ubatr = 0;
int current = 0;
int c_max = 0;
char buffer[50];
int n = 0;

void setup() {
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);
  lora.begin(9600);

  lcd.init();
  lcd.backlight();

  Serial.begin(115200);

  while (CAN_OK != CAN.begin(MCP_ANY, CAN_500KBPS, MCP_8MHZ)) {
    Serial.println("CAN init failed, retrying...");
    delay(100);
  }

  CAN.setMode(MCP_NORMAL);
  Serial.println("CAN init OK");
  
}

void loop() {

  if (CAN.checkReceive() == CAN_MSGAVAIL) {

    unsigned long rxId;
    byte len = 0;
    byte buf[8];

    CAN.readMsgBuf(&rxId, &len, buf);

    switch (rxId) {
      case 0x382 : {
        if (len >= 2) {
          rpm = buf[0] | (buf[1] << 8);
          v = rpm * 0.017;
        };
        break;
      }
      case 0x209 : {
        if (len >= 2) {
          ubat = buf[0] | (buf[1] << 8);
          ubatr = ubat / 16;
        };
        break;
      }
    }
  }

  current_time = millis();
  if (current_time - previous_time >= timing) {
    previous_time = current_time;

    lcd.setCursor(0, 0);
    lcd.print("Vitesse = ");
    lcd.print(v);
    lcd.print(" km/h  ");
    Serial.print("Vitesse = ");
    Serial.println(v);

    lcd.setCursor(0, 1);
    lcd.print("ubat = ");
    lcd.print(ubatr);
    lcd.print(" V  ");
    Serial.print("Tension = ");
    Serial.println(ubatr);

    lcd.setCursor(0, 2);
    lcd.print("current = ");
    lcd.print(current);
    lcd.print(" A  ");
    Serial.print("Current = ");
    Serial.println(current);

    lcd.setCursor(0, 3);
    lcd.print("current_max = ");
    lcd.print(c_max);
    lcd.print(" A ");
    Serial.print("current max = ");
    Serial.println(c_max);

    snprintf(buffer, sizeof(buffer), "%d,%d,%d,%d", n, v, ubatr, current);
    n = n + 1;
    lora.println(buffer);
  }
  if (current_time - previous_time_c >= timing) {
    previous_time_c = current_time;

    current = (analogRead(A0) * 3);
    if (current > c_max) {
      c_max = current;
    };
  }
}