/*
 * AMS_CAN.c
 *
 *  Created on: Mar 19, 2022
 *      Author: jasper
 */

#include "AMS_CAN.h"

#include "BQ_Abstraction_Layer.h"
#include "TMP144.h"

static CAN_HandleTypeDef* handle_ams;
static CAN_HandleTypeDef* handle_car;

void ams_can_init(CAN_HandleTypeDef* ams_handle, CAN_HandleTypeDef* car_handle) {
	handle_ams = ams_handle;
	handle_car = car_handle;

	// Configure filters
	CAN_FilterTypeDef filter_car;
	filter_car.FilterBank = 14;
	filter_car.FilterMode = CAN_FILTERMODE_IDMASK;
	filter_car.FilterScale = CAN_FILTERSCALE_32BIT;
	filter_car.FilterIdHigh = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE >> 16;
	filter_car.FilterIdLow = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE & 0xFFFF;
	filter_car.FilterMaskIdHigh = 0xFFFF;
	filter_car.FilterMaskIdLow = 0xFF00;
	filter_car.FilterFIFOAssignment = CAN_RX_FIFO0;
	filter_car.FilterActivation = ENABLE;
	filter_car.SlaveStartFilterBank = 14;
	if (HAL_CAN_ConfigFilter(handle_car, &filter_car) != HAL_OK) {
		Error_Handler();
	}

	// Start peripheral
	if (HAL_CAN_Start(handle_ams) != HAL_OK) {
		Error_Handler();
	}
	if (HAL_CAN_Start(handle_car) != HAL_OK) {
		Error_Handler();
	}

	// Activate RX notifications
	if (HAL_CAN_ActivateNotification(handle_ams, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
		Error_Handler();
	}
	if (HAL_CAN_ActivateNotification(handle_car, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_ERROR | CAN_IT_LAST_ERROR_CODE) != HAL_OK) {
		Error_Handler();
	}
}

static int cb_triggered = 0;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *handle) {
	static CAN_RxHeaderTypeDef header;
	static uint8_t data[8];
	cb_triggered = 1;

	if (HAL_CAN_GetRxMessage(handle, CAN_RX_FIFO0, &header, data) != HAL_OK) {
		Error_Handler();
	}

	if (handle == handle_ams) {
		ams_can_handle_ams_msg(&header, data);
	} else if (handle == handle_car) {
		ams_can_handle_car_msg(&header, data);
	} else {
		Error_Handler();
	}
}

void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {

}

void ams_can_handle_car_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {

}

void ams_can_send_heartbeat() {
	static CAN_TxHeaderTypeDef header;
	static uint8_t data[8];
	static uint8_t SLAVE_ID = 0;

	header.IDE = CAN_ID_STD;
	header.DLC = 8;
	header.RTR = CAN_RTR_DATA;
	header.TransmitGlobalTime = DISABLE;

	for (int i = 0; i < N_CELLS / 2; i++) {
		header.StdId = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE | (SLAVE_ID) << 4 | i;
		uint16_t v1 = cell_voltages[i * 2];
		uint16_t v2 = cell_voltages[i * 2 + 1];
		data[0] = v1 >> 8;
		data[1] = v1 & 0xFF;
		data[2] = v2 >> 8;
		data[3] = v2 & 0xFF;
		uint16_t t1 = temperatures[i * 2];
		uint16_t t2 = temperatures[i * 2 + 1];
		data[4] = t1 >> 8;
		data[5] = t1 & 0xFF;
		data[6] = t2 >> 8;
		data[7] = t2 & 0xFF;
		uint32_t mailbox;
		HAL_StatusTypeDef status;
		if ((status = HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox)) != HAL_OK) {
			Error_Handler();
		} else {
			HAL_Delay(100);
		}
	}
}
