/*
 * tts.c
 *
 *  Created on: Jun 28, 2024
 *      Author: ted
 */


#include "tts.h"
#include "HTPA_32x32d.h"

// CAN Frame:
#define TTS_CANIDSTART		0x701;		//
CAN_HandleTypeDef* tts_hcan;
CAN_TxHeaderTypeDef tts_canHeader;
uint8_t tts_canData[8];
uint32_t tts_canMailbox;

// Car / Tire info:
TTS_SensorID 	tts_sensorid;
TTS_CarID 		tts_carid;
TTS_TireID 		tts_tireid;
TTS_TireData 	tts_tiredb[4];

void TTS_Init(CAN_HandleTypeDef *hcan) {
	// initialize values
	TTS_LoadTireData();
	tts_sensorid = TTS_FL;
	tts_carid = FT24;
	tts_tireid = OZ7_SLICKS;

	// init CAN
	tts_hcan = hcan;

	// set CAN filter
	/*
	CAN_FilterTypeDef canfilterconfig;
	canfilterconfig.FilterActivation = CAN_FILTER_ENABLE;
	canfilterconfig.FilterBank = 0;
	canfilterconfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
	canfilterconfig.FilterIdHigh = 0x704<<5;
	canfilterconfig.FilterIdLow = 0x700<<5;
	canfilterconfig.FilterMaskIdHigh = 0x704<<5;
	canfilterconfig.FilterMaskIdLow = 0x700<<5;
	canfilterconfig.FilterMode = CAN_FILTERMODE_IDMASK;
	canfilterconfig.FilterScale = CAN_FILTERSCALE_32BIT;
	HAL_CAN_ConfigFilter(tts_hcan, &canfilterconfig);
	*/



	// init CAN_Tx Frame
	//uint8_t canID = tts_sensorid + TTS_CANIDSTART;
	tts_canHeader.IDE = CAN_ID_STD;
	tts_canHeader.StdId = 0x701;
	tts_canHeader.DLC = 8;
	tts_canHeader.RTR = CAN_RTR_DATA;
	for(uint8_t i=0; i<8; i++) {
		tts_canData[i] = 0xFF;
	}

}

void TTS_SendCAN(uint32_t tireZones[5]) {
	// Outer left:
	tts_canData[0] = tireZones[0] & 0xFF;
	tts_canData[1] = (tireZones[0] >> 8) & 0xF;
	// Center left:
	tts_canData[1] = tts_canData[1] | ((tireZones[1] & 0xF) << 4);
	tts_canData[2] = (tireZones[1] >> 4) & 0xFF;
	// Center:
	tts_canData[3] = tireZones[2] & 0xFF;
	tts_canData[4] = (tireZones[2] >> 8) & 0xF;
	// Center right:
	tts_canData[4] = tts_canData[4] | ((tireZones[3] & 0xF) << 4);
	tts_canData[5] = (tireZones[3] >> 4) & 0xFF;
	// Center right:
	tts_canData[6] = tireZones[4] & 0xFF;
	tts_canData[7] = (tireZones[4] >> 8) & 0xF;
	// current tire selected:
	//tts_canData[7] = tts_canData[7] | ((tts_tireid & 0xF) << 4);

	if(HAL_CAN_AddTxMessage(tts_hcan, &tts_canHeader, tts_canData, &tts_canMailbox) != HAL_OK) {
		Error_Handler();
	}

}

void TTS_TireZones(uint32_t tempArray[32], uint32_t tireTempArray[5]) {
	for(uint8_t i = 0; i < 5; i++) {
		tireTempArray[i] = 0;
	}
	uint8_t zoneWidth[5] = {0};
	uint8_t tireid = tts_tireid;

	for(uint8_t i = 0; i < 32; i++) {
		// outer right:
		if((i <= tts_tiredb[tts_tireid].outerRightStart) && (i >= tts_tiredb[tts_tireid].outerRightStop)) {
			tireTempArray[4] = tireTempArray[4] + tempArray[i];
			zoneWidth[4]++;
		}
		// center right:
		if((i <= tts_tiredb[tts_tireid].centerRightStart) && (i >= tts_tiredb[tts_tireid].centerRightStop)) {
			tireTempArray[3] = tireTempArray[3] + tempArray[i];
			zoneWidth[3]++;
		}
		// center:
		if((i <= tts_tiredb[tts_tireid].centerStart) && (i >= tts_tiredb[tts_tireid].centerStop)) {
			tireTempArray[2] = tireTempArray[2] + tempArray[i];
			zoneWidth[2]++;
		}
		// center left:
		if((i <= tts_tiredb[tts_tireid].centerLeftStart) && (i >= tts_tiredb[tts_tireid].centerLeftStop)) {
			tireTempArray[1] = tireTempArray[1] + tempArray[i];
			zoneWidth[1]++;
		}
		// outer left:
		if((i <= tts_tiredb[tts_tireid].outerLeftStart) && (i >= tts_tiredb[tts_tireid].outerLeftStop)) {
			tireTempArray[0] = tireTempArray[0] + tempArray[i];
			zoneWidth[0]++;
		}
	}

	tireTempArray[4] = tireTempArray[4] / zoneWidth[4];
	tireTempArray[3] = tireTempArray[3] / zoneWidth[3];
	tireTempArray[2] = tireTempArray[2] / zoneWidth[2];
	tireTempArray[1] = tireTempArray[1] / zoneWidth[1];
	tireTempArray[0] = tireTempArray[0] / zoneWidth[0];

}

void TTS_LoadTireData(void) {
	tts_tiredb[UNKNOWN].id = UNKNOWN;
	tts_tiredb[UNKNOWN].epsilon = 84;
	tts_tiredb[UNKNOWN].outerLeftStart = 31;
	tts_tiredb[UNKNOWN].outerLeftStop = 26;
	tts_tiredb[UNKNOWN].centerLeftStart = 25;
	tts_tiredb[UNKNOWN].centerLeftStop = 20;
	tts_tiredb[UNKNOWN].centerStart = 19;
	tts_tiredb[UNKNOWN].centerStop = 12;
	tts_tiredb[UNKNOWN].centerRightStart = 11;
	tts_tiredb[UNKNOWN].centerRightStop = 6;
	tts_tiredb[UNKNOWN].outerRightStart = 5;
	tts_tiredb[UNKNOWN].outerRightStop = 0;

	tts_tiredb[OZ7_SLICKS].id = OZ7_SLICKS;
	tts_tiredb[OZ7_SLICKS].epsilon = 84;
	tts_tiredb[OZ7_SLICKS].outerLeftStart = 27;
	tts_tiredb[OZ7_SLICKS].outerLeftStop = 25;
	tts_tiredb[OZ7_SLICKS].centerLeftStart = 24;
	tts_tiredb[OZ7_SLICKS].centerLeftStop = 19;
	tts_tiredb[OZ7_SLICKS].centerStart = 18;
	tts_tiredb[OZ7_SLICKS].centerStop = 13;
	tts_tiredb[OZ7_SLICKS].centerRightStart = 12;
	tts_tiredb[OZ7_SLICKS].centerRightStop = 7;
	tts_tiredb[OZ7_SLICKS].outerRightStart = 6;
	tts_tiredb[OZ7_SLICKS].outerRightStop = 4;

	tts_tiredb[OZ7_RAIN].id = OZ7_RAIN;
	tts_tiredb[OZ7_RAIN].epsilon = 84;
	tts_tiredb[OZ7_RAIN].outerLeftStart = 31;
	tts_tiredb[OZ7_RAIN].outerLeftStop = 26;
	tts_tiredb[OZ7_RAIN].centerLeftStart = 25;
	tts_tiredb[OZ7_RAIN].centerLeftStop = 20;
	tts_tiredb[OZ7_RAIN].centerStart = 19;
	tts_tiredb[OZ7_RAIN].centerStop = 12;
	tts_tiredb[OZ7_RAIN].centerRightStart = 11;
	tts_tiredb[OZ7_RAIN].centerRightStop = 6;
	tts_tiredb[OZ7_RAIN].outerRightStart = 5;
	tts_tiredb[OZ7_RAIN].outerRightStop = 0;

	tts_tiredb[JP8_SLICKS].id = JP8_SLICKS;
	tts_tiredb[JP8_SLICKS].epsilon = 84;
	tts_tiredb[JP8_SLICKS].outerLeftStart = 28;
	tts_tiredb[JP8_SLICKS].outerLeftStop = 24;
	tts_tiredb[JP8_SLICKS].centerLeftStart = 23;
	tts_tiredb[JP8_SLICKS].centerLeftStop = 19;
	tts_tiredb[JP8_SLICKS].centerStart = 18;
	tts_tiredb[JP8_SLICKS].centerStop = 13;
	tts_tiredb[JP8_SLICKS].centerRightStart = 12;
	tts_tiredb[JP8_SLICKS].centerRightStop = 8;
	tts_tiredb[JP8_SLICKS].outerRightStart = 7;
	tts_tiredb[JP8_SLICKS].outerRightStop = 3;
}