#include "slave_monitoring.h"

#include "main.h"
#include "ts_state_machine.h"

#include "FT_CAN_AL.h"

#include "stm32f3xx_hal.h"
#include "stm32f3xx_hal_gpio.h"

#include <stdint.h>
#include <string.h>

SlaveHandle slaves[N_SLAVES];
uint16_t min_voltage;
int16_t max_temp;

void slaves_init() {
  for (int i = 0; i < N_SLAVES; i++) {
    slaves[i].id = i;
    slaves[i].error.kind = SLAVE_ERR_NONE;
    slaves[i].last_message = 0;
    memset(&slaves[i].voltages, 0, sizeof(slaves[i].voltages));
    memset(&slaves[i].temperatures, 0, sizeof(slaves[i].temperatures));
  }

  HAL_GPIO_WritePin(SLAVE_POWER_0_GPIO_Port, SLAVE_POWER_0_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SLAVE_POWER_1_GPIO_Port, SLAVE_POWER_1_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(SLAVE_POWER_DSEL_GPIO_Port, SLAVE_POWER_DSEL_Pin,
                    GPIO_PIN_RESET);
  // TODO: Enable & read out current
  HAL_GPIO_WritePin(SLAVE_POWER_DEN_GPIO_Port, SLAVE_POWER_DEN_Pin,
                    GPIO_PIN_RESET);
}

void slaves_check() {
  int any_slave_error = 0;
  uint32_t now = HAL_GetTick();
  uint16_t min_voltage_new = 0xFFFF;
  int16_t max_temp_new = 0xFFFF;
  for (int i = 0; i < N_SLAVES; i++) {
    if (now - slaves[i].last_message >= SLAVE_TIMEOUT) {
      // Don't overwrite a different error kind
      if (slaves[i].error.kind == SLAVE_ERR_NONE) {
        slaves[i].error.kind = SLAVE_ERR_TIMEOUT;
      }
    } else if (slaves[i].error.kind == SLAVE_ERR_TIMEOUT) {
      slaves[i].error.kind = SLAVE_ERR_NONE;
    }
    for (int j = 0; j < N_CELLS_SERIES; j++) {
      uint16_t v = slaves[i].voltages[j];
      if (v < min_voltage_new) {
        min_voltage_new = v;
      }
    }
    for (int j = 0; j < N_TEMP_SENSORS; j++) {
      int16_t t = slaves[i].temperatures[j];
      if (t > max_temp_new) {
        max_temp_new = t;
      }
    }

    if (slaves[i].error.kind != SLAVE_ERR_NONE) {
      any_slave_error = 1;
    }
  }
  min_voltage = min_voltage_new;
  max_temp = max_temp_new;

  if (any_slave_error) {
    ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, 1);
  }
}

void slaves_handle_panic(const uint8_t *data) {
  uint8_t slave_id = ftcan_unmarshal_unsigned(&data, 1);
  uint8_t error_kind = ftcan_unmarshal_unsigned(&data, 1);
  switch (error_kind) {
  case SLAVE_PANIC_OT:
    slaves[slave_id].error.kind = SLAVE_ERR_OT;
    break;
  case SLAVE_PANIC_UT:
    slaves[slave_id].error.kind = SLAVE_ERR_UT;
    break;
  case SLAVE_PANIC_OV:
    slaves[slave_id].error.kind = SLAVE_ERR_OV;
    break;
  case SLAVE_PANIC_UV:
    slaves[slave_id].error.kind = SLAVE_ERR_UV;
    break;
  }
  slaves[slave_id].error.data = ftcan_unmarshal_unsigned(&data, 4);
  slaves[slave_id].last_message = HAL_GetTick();
}

void slaves_handle_log(const uint8_t *data) {
  // TODO
}
