/*
 *          can.c
 *  Created on: Mai 23, 2024
 *      Author: Hamza
 */

#include "can.h"

//#define CAN_ID_IN   0x501
//#define CAN_ID_OUT  0x502
static uint32_t can_delay_manager = 0;
void can_init(CAN_HandleTypeDef* hcan) { 
  ftcan_init(hcan); 
  ftcan_add_filter(CAN_ID_IN, 0xFFF);
}

/*
This function sends the status of the mvbms, the battery and of powerground.
once every 1s in states: INACTIVE, PRECHARGE, DISCHARGE, CHARGING, ERROR.
once every 0.5s in states: READY, ACTIVE.
with format of:
CAN Messages:
  Error bit
  MVBMS state
  Powerground Status 0-100%
  Errors
  Battery state of charge 
  Pack Voltage
  Current
  Battery temperature (12 bit) 

  Min/Max. Cell Temp (ID, Min Temp, ID, Max Temp)(3B), 
  Min/Max Cell Voltage (ID, Min Voltage, ID, Max Voltage)(3B)

bit 0     (1b):   empty
bit 1-3   (3b):   state
bit 4-11  (8b):   powerground status
bit 12-19 (8b):   error
bit 20-27 (8b):   state of charge from 0-100%
bit 28-39 (12b):  battery voltage
bit 40-51 (12b):  current measurement
bit 52-63 (12b):  temperature of the cell with highest temperature


bit 0-3   (4b):   ID of the sensor with highest temperature
bit 4-7   (4b):   ID of the sensor with lowest temperataure
bit 8-19  (12b):  temperature of the coldest cell
bit 20-23 (4b):   ID of the cell with the lowest voltage
bit 24-35 (12b):  lowest cell voltage
bit 36-39 (4b):   ID of the cell the the highest voltage
bit 40-51 (12b):  highest cell voltage
bit 52-63 (12b):  empty
*/

void can_handle_send_status() {
  if (can_delay_manager > HAL_GetTick())
    return;
  else
    can_delay_manager = HAL_GetTick() + CAN_STATUS_FREQ;
  
  uint8_t data[8] = {};
  int8_t id_highest_temp = -1;
  int16_t highest_temp = INT16_MIN;
  sm_check_battery_temperature(&id_highest_temp, &highest_temp);

  data[0] = ((state.current_state << 4) | (powerground_status >> 4));               // 1 bit emptyy | 3 bit state | 4 bit powerground 
  data[1] = ((powerground_status << 4) | (state.error_source >> 4));              // 4 bit powerground | 4 bit error 
  data[2] = ((state.error_source << 4) | (0));                                      // 4 bit error | 4 bit state of charge
  data[3] = ((0) + (RELAY_BAT_SIDE_VOLTAGE >> 12));                                // 4 bit state of charge | 4 bit battery voltage
  data[4] = ((RELAY_BAT_SIDE_VOLTAGE >> 4));
  data[5] = ((CURRENT_MEASUREMENT >> 8));
  data[6] = ((CURRENT_MEASUREMENT & 0x00F0) | (highest_temp >> 12));
  data[7] = ((highest_temp) >> 4);
  
  ftcan_transmit(CAN_ID_OUT, data, sizeof(data));

  int8_t id_lowest_temp = -1;
  int16_t lowest_temp = INT16_MIN;
  for (int i = 0; i < N_TEMP_SENSORS; i++) {
    if (tmp1075_temps[i] < lowest_temp){
      id_lowest_temp = i;
      lowest_temp = tmp1075_temps[i];
    }
  }

  int8_t id_lowest_volt = -1;
  int16_t lowest_volt = INT16_MIN;
  int8_t id_highest_volt = -1;
  int16_t highest_volt = INT16_MIN;

  for (int i = 0; i < module.sumOfCellMeasurements; i++) {
    if (sm_return_cell_voltage(i) < lowest_temp){
      id_lowest_volt = i;
      lowest_volt = sm_return_cell_voltage(i);
    }
    if (sm_return_cell_voltage(i) > highest_temp){
      id_highest_volt = i;
      highest_volt = sm_return_cell_voltage(i);
    }
  }

  data[0] = ((id_highest_temp & 0x0F) << 4 | (id_lowest_temp & 0x0F));
  data[1] = ((lowest_temp) >> 8);
  data[2] = ((lowest_temp & 0x00F0) | (id_lowest_volt & 0x0F));
  data[3] = (lowest_volt >> 8);
  data[4] = ((lowest_volt & 0x00F0) | (id_highest_volt & 0x0F));
  data[5] = ((highest_volt >> 8));
  data[6] = ((highest_volt & 0x00F0));
  data[7] = 0;
  
  ftcan_transmit(CAN_ID_OUT, data, sizeof(data));
}

/*
can_handle_recieve_command() should only check if the message is valid and then hand it
to the sm_handle_ams_in() which handles the state machine transition.

This function recieves a command from the Autobox with the CAN ID of 0x501.
with format of:
data[0] = target state 
  0x0 STATE_INACTIVE  | disconnect power to the ESC of powerground. Send it to return the mvbms to idle/monitoring mode.  If data[1] != 0 -> assume bad CAN message.
  0x1 STATE_READY     | conneect power to the ESC of powerground and but with no PWM signal.                              If data[1] != 0 -> assume bad CAN message.
  0x2 STATE_ACTIVE    | activate powerground at (data[1]) percent.                                                        If data[1] > 100 -> assume bad CAN message.            

allowed transitions:
  STATE_INACTIVE  ->  STATE_READY
  STATE_READY     ->  STATE_INACTIVE, STATE_ACTIVE
  STATE_ACTIVE    ->  STATE_INACTIVE, STATE_READY
*/
void can_handle_recieve_command(const uint8_t *data){
  if (data[0] == 0x00 && data[1] == 0x00){
    sm_handle_ams_in(data);
  } else if (data[0] == 0x01 && data[1] == 0x00){
    sm_handle_ams_in(data);
  } else if (data[0] == 0x02 && data[1] <= 100) { 
    sm_handle_ams_in(data);
  }
}

/*
implements the _weak method ftcan_msg_recieved_cb() which throws an interrupt when a CAN message is recieved.
it only checks if the id is and datalen is correct thans hands data over to can_handle_recieve_command().

in MXCUBE under CAN NVIC settings "USB low priority or CAN_RX0 interrupts" has to be on 
*/
void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data){
  if (id == 0x501 && datalen == 2){
    can_handle_recieve_command(data);
  }
}