/*
FT_2018_STW_CAN.cpp
*/

#include "FT_2018e_STW_CAN.h"
#include "Arduino.h"
#include "DueTimer.h"
#include "FT18e_STW_INIT.h"
#include "due_can.h"

CAN_FRAME can_0_msg;
// can_1_msg.id = 0x110;
int can_0_temp_data = 0;
int leds[] = {led1, led2,  led3,  led4,  led5,  led6,  led7,  led8,
              led9, led10, led11, led12, led13, led14, led15, led16};

void Init_Can_0() {
  Serial.begin(9600);
  Can0.begin(1000000);   // set CAN0 baud to 1kbit/s and don`t use enable pin!
  Can0.setNumTXBoxes(1); // reserves mailbox 0 for tx only 8 mailboxes are
                         // available (the other 7 mailboxes are for rx)
  Can0.watchFor(CAN_CELL_STATS_ID);
  Can0.watchFor(CAN_BATTERY_STATS_ID);
  Can0.watchFor(CAN_COOLING_STATS_ID);
  Can0.watchFor(CAN_INVERTER_STATS_ID);
  Can0.setGeneralCallback(Receive_Can_0);
  Timer3.attachInterrupt(Send_0x110); // set send interrupt
  Timer3.start(10000);                // Calls every 10ms
}

void Send_0x110() {
  read_buttons();
  read_rotary();
  can_0_msg.id = 0x110;
  can_0_msg.fid = 0;
  can_0_msg.rtr = 0;
  can_0_msg.priority = 0;
  can_0_msg.length = 2;
  can_0_msg.extended = 0;
  can_0_temp_data = 0;
  can_0_temp_data |= Stw_data.button_ll << 0;
  can_0_temp_data |= Stw_data.button_lr << 1;
  can_0_temp_data |= Stw_data.button_rl << 2;
  can_0_temp_data |= Stw_data.button_rr << 3;
  can_0_msg.data.byte[0] = can_0_temp_data;
  can_0_msg.data.byte[1] = Stw_data.mode;
  Can0.sendFrame(can_0_msg);
}

void Receive_Can_0(CAN_FRAME *temp_message) {
  switch (temp_message->id) {
  case CAN_CELL_STATS_ID:
    process_cell_stats(temp_message);
    break;
  case CAN_BATTERY_STATS_ID:
    process_battery_stats(temp_message);
    break;
  case CAN_COOLING_STATS_ID:
    process_cooling_stats(temp_message);
    break;
  case CAN_INVERTER_STATS_ID:
    process_inverter_stats(temp_message);
    break;
  default:
    // TODO: How to handle this in the car?
    Serial.print("ERROR: Unknown CAN ID: ");
    Serial.println(temp_message->id);
  }
}

void process_cell_stats(CAN_FRAME *frame) {
  CellStats *data = (CellStats *)&frame->data;
  Vehicle_data.t_cell_max = data->max_cell_temp;
  Vehicle_data.u_cell_min = data->min_cell_voltage;
}

void process_battery_stats(CAN_FRAME *frame) {
  BatteryStats *data = (BatteryStats *)&frame->data;
  Vehicle_data.u_batt = data->pre_air_voltage;
}

void process_cooling_stats(CAN_FRAME *frame) {
  CoolingStats *data = (CoolingStats *)&frame->data;
  Vehicle_data.p_wat = data->water_pressure;
  Vehicle_data.t_wat = data->water_temp;
  Vehicle_data.t_mot_l = data->motor_l_temp;
  Vehicle_data.t_mot_r = data->motor_r_temp;
}

void process_inverter_stats(CAN_FRAME *frame) {
  InverterStats *data = (InverterStats *)&frame->data;
  uint8_t status = data->status;
  Vehicle_data.inverter.ready = status & CAN_INVERTER_STATS_READY;
  Vehicle_data.inverter.derating = status & CAN_INVERTER_STATS_DERATING;
  Vehicle_data.inverter.warning = status & CAN_INVERTER_STATS_WARNING;
  Vehicle_data.inverter.error = status & CAN_INVERTER_STATS_ERROR;
  Vehicle_data.inverter.on = status & CAN_INVERTER_STATS_ON;
  Vehicle_data.inverter.precharge = status & CAN_INVERTER_STATS_PRECHARGE;
  Vehicle_data.inverter.ams_emerg = status & CAN_INVERTER_STATS_AMS_EMERG;
  Vehicle_data.inverter.ts_active = status & CAN_INVERTER_STATS_TS_ACTIVE;
  Vehicle_data.t_inv = data->temp;
  Vehicle_data.revol = data->velocity;
  Vehicle_data.wheel_speed = data->wheel_speed;
}

void update_LED() {
  bool t_mot = (Vehicle_data.t_mot_l > LED_THRESH_T_MOT) ||
               (Vehicle_data.t_mot_r > LED_THRESH_T_MOT);
  bool t_inv = Vehicle_data.t_inv > LED_THRESH_T_INV;
  bool t_bat = Vehicle_data.t_cell_max > LED_THRESH_T_BAT;

  bool precharge_active = !Vehicle_data.inverter.precharge;
  bool derating = Vehicle_data.inverter.derating;
  bool u_batt = Vehicle_data.u_cell_min < LED_THRESH_U_BATT;

  digitalWrite(led11, t_mot); // rot,		links, oben
  digitalWrite(led12, t_inv); // rot,		links, mitte
  digitalWrite(led13, t_bat); // rot,		links, unten

  digitalWrite(led14, precharge_active); // rot,		rechts, oben
  digitalWrite(led15, derating);         // rot		rechts, mitte
  digitalWrite(led16, u_batt);           // blau		rechts, unten

  bool rev_lim = Vehicle_data.rev_lim;

  int16_t rev = Vehicle_data.revol;
  digitalWrite(led1, RPM_THRESH_1 <= rev);
  digitalWrite(led2, RPM_THRESH_2 <= rev);
  digitalWrite(led3, RPM_THRESH_3 <= rev);
  digitalWrite(led4, RPM_THRESH_4 <= rev);
  digitalWrite(led5, RPM_THRESH_5 <= rev);
  digitalWrite(led6, RPM_THRESH_6 <= rev);
  digitalWrite(led7, RPM_THRESH_7 <= rev);
  digitalWrite(led8, RPM_THRESH_8 <= rev);
  digitalWrite(led9, RPM_THRESH_9 <= rev);
  digitalWrite(led10, RPM_THRESH_10 <= rev);
}