//##############################################################################################################

// Volkswagen Dashboard CAN BUS
// Arduino Sketch v0.1
// (C) by Niklas Schütrumpf 2018

//##############################################################################################################


// https://github.com/Flori1989/MCP2515_lib
#include "Arduino.h"
#include <mcp_can.h>
#include <SPI.h>
#include <ArduinoJson.h>

#define lo8(x) ((int)(x)&0xff)
#define hi8(x) ((int)(x)>>8)

int high_beam = A3;
int fog_beam = A2;
int park_brake = A0;

String inputString = "";
boolean stringComplete = false;

//Variables (Dashboard Functions)
int turning_lights = 3;
boolean turning_lights_blinking = true;
int turning_lights_counter = 0;
int temp_turning_lights = 0;
boolean backlight = false;
int oilpswitch = A1;
int pack_counter = 0;
boolean add_distance = false;
int drive_mode = 0;
int distance_multiplier = 2;
boolean oil_pressure_simulation = true;
boolean door_open = false;
boolean clutch_control = false;
int temp_clutch_control = 0;
boolean check_lamp = false;
int temp_check_lamp = 0;
boolean trunklid_open = false;
int temp_trunklid_open = 0;
boolean battery_warning = false;
int temp_battery_warning = 0;
boolean keybattery_warning = false;
int temp_keybattery_warning = 0;
int lightmode = 0;
boolean seat_belt = false;
int temp_seat_belt = 0;
int engine_control = 0;
int temp_dpf_warning = 0;
boolean dpf_warning = false;
boolean light_fog = false;
int temp_light_fog = 0;
boolean light_highbeam = false;
int temp_light_highbeam = 0;
boolean signal_abs = false;
boolean signal_offroad = false;
boolean signal_handbrake = false;
boolean signal_lowtirepressure = false;
boolean signal_dieselpreheat = false;
boolean signal_watertemp = false;
int temp_signal_abs = 0;
int temp_signal_offroad = 0;
int temp_signal_handbrake = 0;
int temp_signal_lowtirepressure = 0;
int temp_signal_dieselpreheat = 0;
int temp_signal_watertemp = 0;

//Variables (Speed and RPM)
int speed = 0;
int speedTmp = 0;
byte speedL = 0;
byte speedH = 0;

int rpm = 0;
short tempRPM = 0;
byte rpmL = 0;
byte rpmH = 0;

int distance_adder = 0;
int distance_counter = 0;

const int SPI_CS_PIN = 10;
MCP_CAN CAN(SPI_CS_PIN);

//Setup Configuration
void setup()
{
  pinMode(high_beam, OUTPUT);
  pinMode(fog_beam, OUTPUT);
  pinMode(park_brake, OUTPUT);
  pinMode(oilpswitch, OUTPUT);

  digitalWrite(park_brake, HIGH);
  digitalWrite(high_beam, LOW);
  digitalWrite(fog_beam, LOW);

  Serial.begin(115200);
  while (!Serial) continue;

  //Begin with CAN Bus Initialization
START_INIT:

  if (CAN_OK == CAN.begin(CAN_500KBPS, MCP_16MHz))                  // init can bus : baudrate = 500k
  {
    Serial.println("CAN BUS Shield init ok!");
  }
  else
  {
    Serial.println("CAN BUS Shield init fail");
    Serial.println("Init CAN BUS Shield again");
    delay(100);
    //goto START_INIT;
  }

  //##############################################################################################################

  //MAIN SETUP FOR OPERATION

  //Set constant speed and RPM to show on Dashboard
  speed = 0;                        //Set the speed in km/h
  rpm = 0;                        //Set the rev counter
  Fuel(100);                          //Set the fuel gauge in percent

  //Set Dashboard Functions
  backlight = true;                  //Turn the automatic dashboard backlight on or off
  turning_lights = 0;                //Turning Lights: 0 = none, 1 = left, 2 = right, 3 = both
  turning_lights_blinking = true;    //Choose the mode of the turning lights (blinking or just shining)
  add_distance = false;              //Dashboard counts the kilometers (can't be undone)
  distance_multiplier = 2;           //Sets the refresh rate of the odometer (Standard 2)
  signal_abs = false;                //Shows ABS Signal on dashboard
  signal_offroad = false;            //Simulates Offroad drive mode
  signal_handbrake = false;          //Enables handbrake
  signal_lowtirepressure = false;    //Simulates low tire pressure
  oil_pressure_simulation = false;    //Set this to true if dashboard starts to beep
  door_open = false;                 //Simulate open doors
  clutch_control = false;            //Displays the Message "Kupplung" (German for Clutch) on the dashboard's LCD
  check_lamp = false;                //Show 'Check Lamp' Signal on dashboard. B00010000 = on, B00000 = off
  trunklid_open = false;             //Simulate open trunk lid (Kofferraumklappe). B00100000 = open, B00000 = closed
  battery_warning = false;           //Show Battery Warning.
  keybattery_warning = false;        //Show message 'Key Battery Low' on Display. But just after first start of dashboard.
  light_fog = false;                 //Enable Fog Light
  light_highbeam = false;            //Enable High Beam Light
  seat_belt = false;                 //Switch Seat Betl warning light.
  signal_dieselpreheat = false;      //Simualtes Diesel Preheating
  signal_watertemp = false;          //Simualtes high water temperature
  dpf_warning = false;               //Shows the Diesel particle filter warning signal.


  //##############################################################################################################
}

void CanSend(short address, byte a, byte b, byte c, byte d, byte e, byte f, byte g, byte h)
{
  unsigned char DataToSend[8] = {a, b, c, d, e, f, g, h};
  CAN.sendMsgBuf(address, 0, 8, DataToSend);
}

void Fuel(int amount)
{
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);
  pinMode(8, INPUT);
  pinMode(9, INPUT);

  if (amount >= 87)
  {
    pinMode(A4, OUTPUT);
    digitalWrite(A4, LOW);
    return;
  }
  if (amount >= 75)
  {
    pinMode(A5, OUTPUT);
    digitalWrite(A5, LOW);
    return;
  }
  if (amount >= 62)
  {
    pinMode(3, OUTPUT);
    digitalWrite(3, LOW);
    return;
  }
  if (amount >= 50)
  {
    pinMode(4, OUTPUT);
    digitalWrite(4, LOW);
    return;
  }
  if (amount >= 37)
  {
    pinMode(5, OUTPUT);
    digitalWrite(5, LOW);
    return;
  }
  if (amount >= 25)
  {
    pinMode(6, OUTPUT);
    digitalWrite(6, LOW);
    return;
  }
  if (amount >= 18)
  {
    pinMode(7, OUTPUT);
    digitalWrite(7, LOW);
    return;
  }
  if (amount >= 12)
  {
    pinMode(8, OUTPUT);
    digitalWrite(8, LOW);
    return;
  }
  if (amount >= 0)
  {
    pinMode(9, OUTPUT);
    digitalWrite(9, LOW);
    return;
  }
}

//Loop
void loop()
{
  if (stringComplete) {
    Serial.print(inputString.length());
    Serial.print(": ");
    Serial.println(inputString);

    if(inputString.length() >= 39 && inputString.length() <= 46) {
      if(getValue(inputString, ',', 19) != "") {
        speed = getValue(inputString, ',', 0).toInt();
        rpm = getValue(inputString, ',', 1).toInt();
        Fuel(getValue(inputString, ',', 2).toInt());
        
        backlight = getValue(inputString, ',', 3).toInt();
  
        if(getValue(inputString, ',', 4).toInt() == 1 && getValue(inputString, ',', 5).toInt() == 1) {
          turning_lights = 3;
        } else if (getValue(inputString, ',', 4).toInt()) {
          turning_lights = 1;
        } else if (getValue(inputString, ',', 5).toInt()) {
          turning_lights = 2;
        } else {
          turning_lights = 0;
        }
  
        signal_offroad = getValue(inputString, ',', 6).toInt();
        signal_watertemp = getValue(inputString, ',', 7).toInt();
        seat_belt = getValue(inputString, ',', 8).toInt();
        battery_warning = getValue(inputString, ',', 9).toInt();
        check_lamp = getValue(inputString, ',', 10).toInt();
        door_open = getValue(inputString, ',', 11).toInt();
        light_fog = getValue(inputString, ',', 12).toInt();
        light_highbeam = getValue(inputString, ',', 13).toInt();
        signal_handbrake = getValue(inputString, ',', 14).toInt();
        signal_abs = getValue(inputString, ',', 15).toInt();
        oil_pressure_simulation = getValue(inputString, ',', 16).toInt();
        signal_lowtirepressure = getValue(inputString, ',', 17).toInt();
        clutch_control = getValue(inputString, ',', 18).toInt();
        trunklid_open = getValue(inputString, ',', 29).toInt();

        Serial.print("CHECK:");
        Serial.print(speed);
        Serial.print(" - ");
        Serial.print(rpm);
        Serial.print(" - ");
        Serial.print(light_fog);
        Serial.print(" - ");
        Serial.println(turning_lights);
      } else {
        Serial.println("INVALID STRING!");
      }
    } else {
        Serial.println("INVALID STRING!");
    }
    
    inputString = "";
    stringComplete = false;
  }
  
  //Conversion Speed
  speedTmp = speed / 0.0075; //KMH=1.12 MPH=0.62

  //Conversion Low and High Bytes
  speedL = lo8(speedTmp);
  speedH = hi8(speedTmp);

  tempRPM = rpm * 4;
  rpmL = lo8(tempRPM);
  rpmH = hi8(tempRPM);

  if (add_distance == true)
  {
    distance_adder = speedTmp * distance_multiplier;
    distance_counter += distance_adder;
    if (distance_counter > distance_adder) {
      distance_counter = 0;
    }
  }

  if ( oil_pressure_simulation == true)
  {
    //Set Oil Pressure Switch
    if (rpm > 1500) {
      digitalWrite(oilpswitch, LOW);
    }
    else {
      digitalWrite(oilpswitch, HIGH);
    }
  }

  /*Send CAN BUS Data*/

  //Immobilizer
  CanSend(0x3D0, 0, 0x80, 0, 0, 0, 0, 0, 0);

  //Engine on and ESP enabled
  CanSend(0xDA0, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);

  //Enable Cruise Control
  //CanSend(0x289, 0x00, B00000001, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);

  //Dashboard Functions

  //Turning Light blinking
  if (turning_lights_blinking == true)
  {
    if (turning_lights_counter == 15)
    {
      temp_turning_lights = turning_lights;
      turning_lights_counter = turning_lights_counter + 1;
    }
    if (turning_lights_counter == 30)
    {
      temp_turning_lights = 0;
      turning_lights_counter = 0;
    }
    else
    {
      turning_lights_counter = turning_lights_counter + 1;
    }
  }
  else
  {
    temp_turning_lights = turning_lights;
  }

  //Check main settings

  //DPF
  if (dpf_warning == true) temp_dpf_warning = B00000010;
  else temp_dpf_warning = B00000000;

  //Seat Belt
  if (seat_belt == true) temp_seat_belt = B00000100;
  else temp_seat_belt = B00000000;

  //Battery Warning
  if (battery_warning == true) temp_battery_warning = B10000000;
  else temp_battery_warning = B00000000;

  //Trunk Lid (Kofferraumklappe)
  if (trunklid_open == true) temp_trunklid_open = B00100000;
  else temp_trunklid_open = B00000000;

  //Check Lamp Signal
  if (check_lamp == true) temp_check_lamp = B00010000;
  else temp_check_lamp = B00000000;

  //Clutch Text on LCD
  if (clutch_control == true) temp_clutch_control = B00000001;
  else temp_clutch_control = B00000000;

  //Warning for low key battery
  if (keybattery_warning == true) temp_keybattery_warning = B10000000;
  else temp_keybattery_warning = B00000000;

  //Lightmode Selection (Fog Light and/or High Beam)
  if (light_highbeam == true) temp_light_highbeam = B01000000;
  else temp_light_highbeam = B00000000;
  if (light_fog == true) temp_light_fog = B00100000;
  else temp_light_fog = B00000000;
  lightmode = temp_light_highbeam + temp_light_fog;

  //Engine Options (Water Temperature, Diesel Preheater)
  if (signal_dieselpreheat == true) temp_signal_dieselpreheat = B00000010;
  else temp_signal_dieselpreheat = B00000000;
  if (signal_watertemp == true) temp_signal_watertemp = B00010000;
  else temp_signal_watertemp = B00000000;
  engine_control = temp_signal_dieselpreheat + temp_signal_watertemp;

  //Drivemode Selection (ABS, Offroad, Low Tire Pressure, handbrake)
  if (signal_abs == true) temp_signal_abs = B0001;
  else temp_signal_abs = B0000;
  if (signal_offroad == true) temp_signal_offroad = B0010;
  else temp_signal_offroad = B0000;
  if (signal_handbrake == true) temp_signal_handbrake = B0100;
  else temp_signal_handbrake = B0000;
  if (signal_lowtirepressure == true) temp_signal_lowtirepressure = B1000;
  else temp_signal_lowtirepressure = B0000;

  drive_mode = temp_signal_abs + temp_signal_offroad + temp_signal_handbrake + temp_signal_lowtirepressure;


  //Send CAN data every 200ms
  pack_counter++;
  if (pack_counter == 20)
  {
    //Turning Lights 2
    CanSend(0x470, temp_battery_warning + temp_turning_lights, temp_trunklid_open + door_open, backlight, 0x00, temp_check_lamp + temp_clutch_control, temp_keybattery_warning, 0x00, lightmode);

    //Diesel engine
    CanSend(0x480, 0x00, engine_control, 0x00, 0x00, 0x00, temp_dpf_warning, 0x00, 0x00);

    ///Engine
    //CanSend(0x388, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);


    //Cruise Control
    //CanSend(0x289, 0x00, B00000101, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);

    pack_counter = 0;
  }

  //Motorspeed
  CanSend(0x320, 0x00, (speedL * 100), (speedH * 100), 0x00, 0x00, 0x00, 0x00, 0x00);

  //RPM
  CanSend(0x280, 0x49, 0x0E, rpmL, rpmH, 0x0E, 0x00, 0x1B, 0x0E);

  //Speed
  CanSend(0x5A0, 0xFF, speedL, speedH, drive_mode, 0x00, lo8(distance_counter), hi8(distance_counter), 0xad);

  //ABS
  CanSend(0x1A0, 0x18, speedL, speedH, 0x00, 0xfe, 0xfe, 0x00, 0xff);

  //Airbag
  CanSend(0x050, 0x00, 0x80, temp_seat_belt, 0x00, 0x00, 0x00, 0x00, 0x00);

  //Wait a time
  delay(5);
}

void serialEvent() {
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    if (inChar == ';') {
      stringComplete = true;
    } else {
      inputString += inChar;
    }
  }
}

String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = { 0, -1 };
  int maxIndex = data.length() - 1;
  
  for (int i = 0; i <= maxIndex && found <= index; i++) {
    if (data.charAt(i) == separator || i == maxIndex) {
      found++;
      strIndex[0] = strIndex[1] + 1;
      strIndex[1] = (i == maxIndex) ? i+1 : i;
    }
  }
  return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}