/*
 * CAN_Communication.c
 *
*  Created on: 24. April, 2024
 *      Author: nived
 */
#include "CAN_Communication.h"
#include "Channel_Control.h"
#include "Current_Monitoring.h"

rx_status_frame rxstate = {};
volatile uint8_t canmsg_received = 0;
extern PortExtenderGPIO EN_Ports;
extern CurrentMeasurements current_measurements_adc_val;

//extern IWDG_HandleTypeDef hiwdg;
extern uint32_t lastheartbeat;
extern int inhibit_SDC;

void can_init(CAN_HandleTypeDef* hcan) {
  ftcan_init(hcan);
  ftcan_add_filter(0x00, 0x00); // No Filter
}

void can_sendloop() {
  static uint8_t additionaltxcouter = 0;

  uint8_t status_data[7];
  status_data[0] = EN_Ports.porta.porta;
  status_data[1] = EN_Ports.portb.portb;
  status_data[2] = rxstate.tsacfans;
  status_data[3] = rxstate.radiatorfans;
  status_data[4] = rxstate.pwmaggregat;
  status_data[5] = rxstate.cooling_pump;
  status_data[6] = !inhibit_SDC; // Now means "WD Okay". TODO: Change DBC
  ftcan_transmit(TX_STATUS_MSG_ID, status_data, 7);

  uint8_t data[8];
return;
  if (additionaltxcouter < 4) {
    switch (additionaltxcouter) {
    case 0:

      data[0] = current_measurements_adc_val.always_on >> 8;
      data[1] = current_measurements_adc_val.always_on & 0xFF;
      data[2] = current_measurements_adc_val.misc >> 8;
      data[3] = current_measurements_adc_val.misc & 0xFF;
      data[4] = current_measurements_adc_val.inverters >> 8;
      data[5] = current_measurements_adc_val.inverters & 0xFF;
      data[6] = current_measurements_adc_val.sdc >> 8;
      data[7] = current_measurements_adc_val.sdc & 0xFF;
      ftcan_transmit(CUR_CHANNELS_1_ID, data, 8);

      break;
    case 1:

      data[0] = current_measurements_adc_val.tsac_fans >> 8;
      data[1] = current_measurements_adc_val.tsac_fans & 0xFF;
      data[2] = current_measurements_adc_val.cooling_pump >> 8;
      data[3] = current_measurements_adc_val.cooling_pump & 0xFF;
      data[4] = current_measurements_adc_val.aggregat >> 8;
      data[5] = current_measurements_adc_val.aggregat & 0xFF;
      data[6] = current_measurements_adc_val.epsc >> 8;
      data[7] = current_measurements_adc_val.epsc & 0xFF;
      ftcan_transmit(CUR_CHANNELS_2_ID, data, 8);

      break;
    case 2:

      data[0] = current_measurements_adc_val.ebsvalve_a >> 8;
      data[1] = current_measurements_adc_val.ebsvalve_a & 0xFF;
      data[2] = current_measurements_adc_val.ebsvalve_b >> 8;
      data[3] = current_measurements_adc_val.ebsvalve_b & 0xFF;
      data[4] = current_measurements_adc_val.ebs_cs_valve >> 8;
      data[5] = current_measurements_adc_val.ebs_cs_valve & 0xFF;
      data[6] = current_measurements_adc_val.gss >> 8;
      data[7] = current_measurements_adc_val.gss & 0xFF;
      ftcan_transmit(CUR_CHANNELS_3_ID, data, 8);

      break;
    case 3:

      data[0] = current_measurements_adc_val.radiator_fans >> 8;
      data[1] = current_measurements_adc_val.radiator_fans & 0xFF;
      data[2] = current_measurements_adc_val.acu >> 8;
      data[3] = current_measurements_adc_val.acu & 0xFF;
      data[4] = current_measurements_adc_val.servos >> 8;
      data[5] = current_measurements_adc_val.servos & 0xFF;
      data[6] = current_measurements_adc_val.lidar >> 8;
      data[7] = current_measurements_adc_val.lidar & 0xFF;
      ftcan_transmit(CUR_CHANNELS_4_ID, data, 8);

      break;
    default:
      break;
    }
    additionaltxcouter++;
  } else {
    additionaltxcouter = 0;
  }
}

void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t* data) {

  canmsg_received = 1;

  if ((id == RX_STATUS_MSG_ID) && (datalen == 7)) {
    rxstate.iostatus.porta.porta    = data[0];
    rxstate.iostatus.portb.portb    = data[1];
    rxstate.radiatorfans            = data[2];
    rxstate.tsacfans                = data[3];
    rxstate.pwmaggregat             = data[4];
    rxstate.cooling_pump            = data[5];
    rxstate.checksum                = data[6];
  }

  if (id == RX_STATUS_HEARTBEAT) {
    lastheartbeat = HAL_GetTick();
    inhibit_SDC = 0;
  }

}