#include "can.h"

#include "main.h"
#include "shunt_monitoring.h"
#include "slave_monitoring.h"
#include "soc_estimation.h"
#include "ts_state_machine.h"

#include "can-halal.h"

#include <stdint.h>

void can_init(CAN_HandleTypeDef *handle) {
  ftcan_init(handle);
  ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);
  ftcan_add_filter(CAN_ID_AMS_IN, 0xFFF);
  ftcan_add_filter(CAN_ID_SLAVE_PANIC, 0xFFF);
  ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
  // TODO: Slave status?
}

HAL_StatusTypeDef can_send_status() {
  uint8_t data[6];
  data[0] = ts_state.current_state;
  data[1] = current_soc;
  ftcan_marshal_unsigned(&data[2], min_voltage, 2);
  ftcan_marshal_signed(&data[4], max_temp, 2);
  return ftcan_transmit(CAN_ID_AMS_STATUS, data, sizeof(data));
}

void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
  if ((id & 0xFF0) == CAN_ID_SHUNT_BASE) {
    shunt_handle_can_msg(id, data);
    return;
  }
  switch (id) {
  case CAN_ID_SLAVE_PANIC:
    slaves_handle_panic(data);
    break;
  case CAN_ID_SLAVE_LOG:
    slaves_handle_log(data);
    break;
  case CAN_ID_AMS_IN:
    ts_sm_handle_ams_in(data);
    break;
  }
}