/*
 * slave_handler.c
 *
 *  Created on: Jun 21, 2023
 *      Author: max
 */

#include "slave_handler.h"
#include "can.h"
#include "can-halal.h"

static uint8_t slave_id_to_index[128] = {0xFF};


void slave_handler_init()
{
	memset(slave_id_to_index,0xFF,128);
}

SlaveHandle slaves[N_SLAVES];
static size_t get_slave_index(uint8_t);

void slaves_handle_status(const uint8_t *data) {
  uint8_t slave_id = data[0] & 0x7F;
  uint8_t idx = get_slave_index(slave_id);
  int error = data[0] & 0x80;
  if (error) {
    if (slaves[idx].error.kind == SLAVE_ERR_NONE) {
      slaves[idx].error.kind = SLAVE_ERR_UNKNOWN;
    }
  } else {
    slaves[idx].error.kind = SLAVE_ERR_NONE;
  }
  slaves[idx].soc = data[1];
  const uint8_t *ptr = &data[2];
  slaves[idx].min_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
  slaves[idx].max_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
  slaves[idx].max_temp = ftcan_unmarshal_unsigned(&ptr, 2);
  slaves[idx].last_message = HAL_GetTick();
}

float slaves_get_maximum_voltage()
{
	float maxvoltage = 0;
	for(uint8_t i = 0; i < N_SLAVES; i++)
	{
		if(maxvoltage < slaves[i].max_voltage)
			maxvoltage = slaves[i].max_voltage;
	}
	return ((float)maxvoltage)/10000;
}



static size_t get_slave_index(uint8_t slave_id) {
  // Slave IDs are 7-bit, so we can use a 128-element array to map them to
  // indices. 0xFF is used to mark unseen slave IDs, since the highest index we
  // could need is N_SLAVES - 1 (i.e. 5).
  static size_t next_slave_index = 0;
  if (slave_id_to_index[slave_id] == 0xFF) {
    if (next_slave_index >= N_SLAVES) {
      // We've seen more than N_SLAVES slave IDs, this shouldn't happen.
      Error_Handler();
    }
    slave_id_to_index[slave_id] = next_slave_index;
    slaves[next_slave_index].id = slave_id;
    next_slave_index++;
  }
  return slave_id_to_index[slave_id];
}