/*
 * Testbench.c
 *
 *  Created on: 13.03.2023
 *      Author: david
 */

#include "Testbench.h"
#include "AMS_CAN.h"
#include "common_defs.h"
#include "AMS_CAN.h"
#include "AMS_HighLevel.h"
#include "stm32f3xx.h"
#include "stm32f3xx_hal.h"
#include "stm32f3xx_hal_can.h"
#include <stdint.h>
#include "ADBMS_Abstraction.h"
#include "main.h"


void canTestSendTemperatures(uint16_t* data){
	static CAN_TxHeaderTypeDef header;

	header.IDE = CAN_ID_STD;
	header.DLC = 8;
	header.RTR = CAN_RTR_DATA;
	header.TransmitGlobalTime = DISABLE;
	uint8_t buffer[24];
	uint8_t tmp[8];

	for(int i = 0; i < 12; i++){
		buffer[((i*2)+1)] = data[i] >> 8;
		buffer[(i*2)] =  data[i];
	}

	for(int i = 0; i < 8; i++){
		tmp[i] = buffer[i];
	}
	if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1, CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
		uint32_t mailbox;
		HAL_CAN_AddTxMessage(ams_can_handle, &header, tmp, &mailbox);
	 }

	int m = 0;
	for(int i = 8; i < 16; i++){
		tmp[m] = buffer[i];
		m++;
	}

	if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1, CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
		uint32_t mailbox;
		HAL_CAN_AddTxMessage(ams_can_handle, &header, tmp, &mailbox);
	 }
	m = 0;
	for(int i = 16; i < 24; i++){
		tmp[m] = buffer[i];
		m++;
	}

	if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1, CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
		uint32_t mailbox;
		HAL_CAN_AddTxMessage(ams_can_handle, &header, tmp, &mailbox);
	 }
}

void canTestSendAnswer(uint8_t* data){
	static CAN_TxHeaderTypeDef header;

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

	  if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
										  CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
		uint32_t mailbox;
		HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
	  }
}

void resetData(uint8_t* data){
	for(int i = 0; i < 8; i++){
		data[0] = 0;
	}

}
void readTemperatures(){
	uint8_t last_error = 0;
	int N_SENSORS = 12;
	uint16_t temperatures[N_SENSORS];
	for (int i = 0; i < N_SENSORS; i++) {
	        if (sensor_read(i, &temperatures[i]) != HAL_OK) {
	          sensor_init(i);
	          last_error = HAL_GetTick();
	        }
	 }
	canTestSendTemperatures(temperatures);
}

void testLoop(uint8_t* data){
	uint8_t action = data[0];
	switch(action){
		case CAN_TEST:
			HAL_Delay(100);
			canTestSendAnswer(data);
			break;
		case VOLTAGE_TEST:
			HAL_Delay(100);
			amsReadCellVoltages(&module);
			ams_can_send_heartbeat();
			break;
		case TEMP_TEST:
			HAL_Delay(1000);
			readTemperatures();
			break;
		case EPROM_TEST:
			HAL_Delay(1000);
			for(uint16_t i = 1; i < 9; i++ ){
				if(i == 4){
					writeeeprom(i*3, 0x42);
				}else{
					writeeeprom(i*3, 0x69);
				}
			}

			HAL_Delay(1000);
			for(uint16_t i = 1; i < 9; i++ ){
				data[i-1]  = readeeprom(i*3);
			}
			canTestSendAnswer(data);
			break;
		case BALANCING_TEST:
			HAL_Delay(1000);
			for(int i = 0; i  < 17; i++){
				amsConfigBalancing(0x00001<<i);
					amsStartBalancing(10);
					HAL_Delay(1000);
			}
			HAL_Delay(1000);
			amsConfigBalancing(0x1FFFF);
			amsStartBalancing(10);
			HAL_Delay(1000);
			amsStopBalancing();
			break;

	}
	resetData(data);
}

