#include "can.h"

#include "imd_monitoring.h"
#include "main.h"
#include "shunt_monitoring.h"
#include "battery.h"
#include "soc_estimation.h"
#include "ts_state_machine.h"

#include "can-halal.h"

#include <math.h>
#include <stdint.h>

void can_init(FDCAN_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_STATUS_BASE, 0xFF0);
  // ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
}

HAL_StatusTypeDef can_send_status() {
  uint8_t data[8];
  data[0] = ts_state.current_state | (sdc_closed << 7);
  data[1] = roundf(current_soc);
  ftcan_marshal_unsigned(&data[2], min_voltage, 2);   //was declared in slave_monitoring.c 
  ftcan_marshal_signed(&data[4], max_temp, 2);
  data[6] = imd_data.state | (imd_data.ok << 7);
  if (imd_data.r_iso < 0xFFF) {
    data[7] = imd_data.r_iso >> 4;
  } else {
    data[7] = 0xFF;
  }
  HAL_StatusTypeDef ret = ftcan_transmit(CAN_ID_AMS_STATUS, data, sizeof(data));
  if (ret != HAL_OK) {
    return ret;
  }
  data[0] = (sdc_closed_nodelay << 0) | (ts_error << 1) | (hv_active << 2) |
            (neg_air_closed << 3) | (pos_air_closed << 4) |
            (precharge_closed << 5) | (pre_and_air_open << 6);
  return ftcan_transmit(CAN_ID_AMS_SIGNALS, data, 1);
}

HAL_StatusTypeDef can_send_error(TSErrorKind kind, uint8_t arg) {
  uint8_t data[2];
  data[0] = kind;
  data[1] = arg;
  return ftcan_transmit(CAN_ID_AMS_ERROR, data, sizeof(data));
}

void ftcan_msg_received_cb(uint16_t id, size_t, const uint8_t *data) {
  if ((id & 0xFF0) == CAN_ID_SHUNT_BASE) {
    shunt_handle_can_msg(id, data);
    return;
  } 
  // else if ((id & 0xFF0) == CAN_ID_SLAVE_STATUS_BASE) {
  //   slaves_handle_status(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;
  // }
}