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

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;

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

int16_t rpm = 0;
int16_t ubat = 0;
int v = 0;
int ubatr = 0;

void setup() {
  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);
  }
}