#include "TMP1075.h"

#define MAX_TEMP        ((int16_t)(59 / 0.0625f))
#define MAX_FAILED_TEMP 12 //TODO: change value for compliance with the actual number of sensors
#warning "change value for compliance with the actual number of sensors"

int16_t tmp1075_temps[N_TEMP_SENSORS] = {0};
uint32_t tmp1075_failed_sensors = 0;
uint8_t nfailed_temp_sensors = 0;

I2C_HandleTypeDef* hi2c;

HAL_StatusTypeDef tmp1075_init(I2C_HandleTypeDef* handle) {
  hi2c = handle;
  for (int i = 0; i < N_TEMP_SENSORS; i++) {
    HAL_StatusTypeDef status = tmp1075_sensor_init(i);
    if (status != HAL_OK) {
      return status;
    }
  }
  return HAL_OK;
}

void handle_over_maxtemp(uint8_t index, uint16_t value) {
  set_error_source(ERROR_SOURCE_TEMPERATURES);
  error_data.data_kind = SEK_OVERTEMP;
  error_data.data[0] = index;
  uint8_t* ptr = &error_data.data[1];
  ptr = ftcan_marshal_unsigned(ptr, value, 2);
}

HAL_StatusTypeDef tmp1075_measure() {
  int err = 0;
  int temp_error = 0;
  for (int i = 0; i < N_TEMP_SENSORS; i++) {
    if (tmp1075_sensor_read(i, &tmp1075_temps[i]) != HAL_OK ||
        (tmp1075_temps[i] & 0x000F) != 0) {
      tmp1075_failed_sensors |= 1 << i;
      nfailed_temp_sensors++;
      err = 1;
    } else {
      tmp1075_temps[i] >>= 4;
      tmp1075_failed_sensors &= ~(1 << i);
      if (tmp1075_temps[i] >= MAX_TEMP) {
        temp_error = 1;
        handle_over_maxtemp(i, tmp1075_temps[i]);
      }
      #warning "check for under temp"
    }
  }
  if (nfailed_temp_sensors > MAX_FAILED_TEMP) {
    error_data.data_kind = SEK_TOO_FEW_TEMPS;
    set_error_source(ERROR_SOURCE_TEMPERATURES);
  } else if (!temp_error) {
    clear_error_source(ERROR_SOURCE_TEMPERATURES);
  }
  nfailed_temp_sensors = 0;
  return err ? HAL_ERROR : HAL_OK;
}

HAL_StatusTypeDef tmp1075_sensor_init(int n) {
  uint16_t addr = (0b1000000 | n) << 1;
  uint8_t data[] = {0};
  return HAL_I2C_Master_Transmit(hi2c, addr, data, sizeof(data), 100);
}

HAL_StatusTypeDef tmp1075_sensor_read(int n, int16_t* res) {
  uint16_t addr = (0b1000000 | n) << 1;
  addr |= 1; // Read
  uint8_t result[2];
  HAL_StatusTypeDef status =
      HAL_I2C_Master_Receive(hi2c, addr, result, sizeof(result), 5); //5ms timeout for failure (cascading faliure max = 30 * 5 = 150ms)
  if (status == HAL_OK) {
    *res = (result[0] << 8) | result[1];
  }
  return status;
}