/*
FT_2018_STW_CAN.cpp
*/

#include "FT_2018_STW_CAN.h"
#include "Arduino.h"
#include "DueTimer.h"
#include "FT18_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() {
  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)

  // We only have 7 mailboxes, but want to receive 9 messages. This trick should
  // allow us to receive BCU_APS_BRAKE, BCU_ETC and BCU_SHIFT_CTRL in the same
  // mailbox. It will also let through 0x506, but that shouldn't be much of an
  // issue.
  Can0.watchFor(CAN_ID_BCU_APS_BRAKE & CAN_ID_BCU_ETC & CAN_ID_BCU_SHIFT_CTRL,
                0x7FC);
  Can0.watchFor(CAN_ID_BCU_TIRES);
  Can0.watchFor(CAN_ID_BCU_LAP_TIME);
  Can0.watchFor(CAN_ID_MS4_IGN_REV_ATH);
  Can0.watchFor(CAN_ID_MS4_SPEED);
  Can0.watchFor(CAN_ID_MS4_ETC);
  Can0.watchFor(CAN_ID_MS4_STATES_TEMP_PRESS);

  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.Stw_shift_up & 0b00000001;
  can_0_temp_data |= Stw_data.Stw_shift_down << 1 & 0b00000010;
  can_0_temp_data |= Stw_data.Stw_neutral << 2 & 0b00000100;
  can_0_temp_data |= Stw_data.Stw_auto_shift << 3 & 0b00001000;
  can_0_temp_data |= Stw_data.buttonStateEnc1 << 5 & 0b00100000; // pitlane
  can_0_msg.data.byte[0] = can_0_temp_data;
  can_0_msg.data.byte[1] = Stw_data.trc & 0b00001111;
  can_0_msg.data.byte[2] = Stw_data.mode & 0b00000111;
  if ((Stw_data.Stw_auto_shift << 3 & 0b00001000)) {
    if (Vehicle_data.g_auto) {
      Vehicle_data.g_auto = false;
    } else {
      Vehicle_data.g_auto = true;
    }
  }
  Can0.sendFrame(can_0_msg);
}

void Receive_Can_0(CAN_FRAME *temp_message) {
  switch (temp_message->id) {
  case CAN_ID_BCU_APS_BRAKE: {
    Vehicle_data.p_brake_front = temp_message->data.byte[1];
    Vehicle_data.p_brake_rear = temp_message->data.byte[2];
    Vehicle_data.drs_active = ((temp_message->data.byte[0] >> 7) & 0b00000001);
    break;
  }
  case CAN_ID_BCU_ETC: { // eDrossel error bit
    Vehicle_data.e_thro = (temp_message->data.byte[0] & 0xF0); // bit 4-7

    if (temp_message->data.byte[0] & 0x80) {
      Stw_data.error_type = 1; //"pc_error";
    }
    if (temp_message->data.byte[0] & 0x40) {
      Stw_data.error_type = 2; //"bse_error";
    }
    if (temp_message->data.byte[0] & 0x20) {
      Stw_data.error_type = 3; //"aps_error";
    }
    if (temp_message->data.byte[0] & 0x10) {
      Stw_data.error_type = 4; //"etb_error";
    }
    break;
  }
  case CAN_ID_BCU_SHIFT_CTRL: { // autoshift+gear
    Vehicle_data.gear = (temp_message->data.byte[1]) >> 5;
    break;
  }
  case CAN_ID_BCU_TIRES: { // Tire temps
    Vehicle_data.t_trl = temp_message->data.byte[1];
    Vehicle_data.t_trr = temp_message->data.byte[4];
    Vehicle_data.t_tfl = temp_message->data.byte[5];
    Vehicle_data.t_tfr = temp_message->data.byte[6];
    break;
  }
  case CAN_ID_BCU_LAP_TIME: { // lap time
    Vehicle_data.lap_time_sec = temp_message->data.byte[1];
    Vehicle_data.lap_time_msec =
        temp_message->data.byte[2] | ((temp_message->data.byte[3] & 0b11) << 8);
    break;
  }
  case CAN_ID_MS4_IGN_REV_ATH: { // rpm
    Vehicle_data.revol =
        (temp_message->data.byte[4] | temp_message->data.byte[3] << 8);
    break;
  }
  case CAN_ID_MS4_SPEED: { // speed
    Vehicle_data.speed_fl = 2 * (temp_message->data.byte[2]);
    Vehicle_data.speed_fr = 2 * (temp_message->data.byte[3]);
    Vehicle_data.speed = (Vehicle_data.speed_fl + Vehicle_data.speed_fr) / 2;
    break;
  }
  case CAN_ID_MS4_ETC: { // battery voltage
    Vehicle_data.u_batt = temp_message->data.byte[6];
    break;
  }
  case CAN_ID_MS4_STATES_TEMP_PRESS: { // temp und p
    if (temp_message->data.byte[0] == 1) {
      Vehicle_data.p_oil = temp_message->data.byte[5];
      Vehicle_data.p_fuel = temp_message->data.byte[7];
    } else if (temp_message->data.byte[0] == 2) {
      Vehicle_data.t_air = temp_message->data.byte[7];
    } else if (temp_message->data.byte[0] == 4) {
      Vehicle_data.t_oil = temp_message->data.byte[5];
    } else if (temp_message->data.byte[0] == 5) {
      Vehicle_data.t_mot = temp_message->data.byte[4];
    }
    break;
  }
  }
}

void update_LED() {
  // Copyright Michael Dietzel
  // m.dietzel@fasttube.de
  // Edit Michael Witt 05-2015
  // m.witt@fasttube.de

  // EDIT BAHA ZARROUKI 05-2107
  // z.baha@fasttube.de

  // alle Werte als Hex-Werte angegeben
  bool t_oil = (Vehicle_data.t_oil - 40) >= 0x96; // 150°C temp.oil
  bool t_air = (Vehicle_data.t_air - 40) >= 0x3C; // 60°C temp.llk
  bool t_mot =
      ((Vehicle_data.t_mot - 40) >= 0x69) and
      ((Vehicle_data.t_mot - 40) != 0xC8); // 105°C temp.water und !=200

  bool g_auto = Vehicle_data.drs_active;
  bool u_batt = Vehicle_data.u_batt <= 0xA9; // 11.95V batt.spann.
  bool e_dros = Vehicle_data.e_thro;         // error-bit

  bool rev_lim = Vehicle_data.rev_lim;

  uint16_t rev = Vehicle_data.revol;

  /*if(Vehicle_data.rev_lim){
          for (int j = 0; j < 10; j++){
                  digitalWrite(leds[j], HIGH);
                  //analogWrite(leds[j], STW_data.br);	//nur eine der zwei
  zeilen
          }
          delay(100);
          for (int j = 0; j < 10; j++){
                  digitalWrite(leds[j], LOW);
          }
          delay(100);
  }else{*/
  /*uint8_t helligkeit = 20;
  if(RPM_THRES_1 <= rev){
          analogWrite(led1, helligkeit);
  }else{
          analogWrite(led1, 0);
  }
  if(RPM_THRES_2 <= rev){
          analogWrite(led2, helligkeit);
  }else{
          analogWrite(led2, 0);
  }
  if(RPM_THRES_3 <= rev){
          analogWrite(led3, helligkeit);
  }else{
          analogWrite(led3, 0);
  }
  if(RPM_THRES_4 <= rev){
          analogWrite(led4, helligkeit);
  }else{
          analogWrite(led4, 0);
  }
  if(RPM_THRES_5 <= rev){
          analogWrite(led5, helligkeit);
  }else{
          analogWrite(led5, 0);
  }
  if(RPM_THRES_6 <= rev){
          analogWrite(led6, helligkeit);
  }else{
          analogWrite(led6, 0);
  }
  if(RPM_THRES_7 <= rev){
          analogWrite(led7, helligkeit);
  }else{
          analogWrite(led7, 0);
  }
  if(RPM_THRES_8 <= rev){
          analogWrite(led8, helligkeit);
  }else{
          analogWrite(led8, 0);
  }
  if(RPM_THRES_9 <= rev){
          analogWrite(led9, helligkeit);
  }else{
          analogWrite(led9, 0);
  }
  if(RPM_THRES_10 <= rev){
          analogWrite(led10, helligkeit);
  }else{
          analogWrite(led10, 0);
  }*/
  digitalWrite(led1, RPM_THRES_1 <= rev);
  digitalWrite(led2, RPM_THRES_2 <= rev);
  digitalWrite(led3, RPM_THRES_3 <= rev);
  digitalWrite(led4, RPM_THRES_4 <= rev);
  digitalWrite(led5, RPM_THRES_5 <= rev);
  digitalWrite(led6, RPM_THRES_6 <= rev);
  digitalWrite(led7, RPM_THRES_7 <= rev);
  digitalWrite(led8, RPM_THRES_8 <= rev);
  digitalWrite(led9, RPM_THRES_9 <= rev);
  digitalWrite(led10, RPM_THRES_10 <= rev);

  digitalWrite(led11, t_mot); // rot,		links, oben
  digitalWrite(led12, t_air); // rot,		links, mitte
  digitalWrite(led13, t_oil); // rot,		links, unten

  digitalWrite(led14, e_dros); // rot,		rechts, oben
  digitalWrite(led15, u_batt); // rot		rechts, mitte
  digitalWrite(led16, g_auto); // blau		rechts, unten
                               /*if(Vehicle_data.g_auto){
                                       digitalWrite(led16, HIGH);
                               }else{
                                       digitalWrite(led16, LOW);
                               }*/
}