/*
 * ASS.c
 *
 *  Created on: Mar 24, 2025
 *      Author: Vincent Chau
 */

#include <stdbool.h>
#include <math.h>
#include "main.h"
#include "ASS.h"
#include "can-halal.h"

// I2C address
#define ASS_SENSOR_ADDRESS			0x6C
#define ASS_CONFIG_ADDRESS			0x22
#define ASS_TEMP_ADDRESS			0x2E
#define ASS_PRESSURE_ADDRESS		0x30
#define ASS_STATUS_SYNC_ADDRESS		0x32
#define ASS_STATUS_ADDRESS			0x36

// I2C transmit delay
#define ASS_I2C_MAX_DELAY			0xFF

I2C_HandleTypeDef* ass_hi2c;	// pointer to i2c handle

// Sensor data
ASS_Status ass_statusReg;
uint8_t data_status[2], data_temp[2], data_pres[2];
int16_t pressure, temperature;

I2C_HandleTypeDef* ass_hi2c;	// pointer to i2c handle

/* initializing sensor
		1. wakeup by writing to adress
*/
void ASS_Init(I2C_HandleTypeDef *hi2c){
	ass_hi2c = hi2c;

	uint8_t wakeup[] = {0x69, 0xB1};
	ASS_WriteRegister(ASS_CONFIG_ADDRESS, *wakeup);
}

/* reads pressure
 * 		- reads pressure and status register
 * 		- calculates pressure while checking status flag of read */

void ASS_ReadSensor(int16_t* data_pressure, int16_t* data_temperature) {
	ASS_ReadRegister(ASS_TEMP_ADDRESS, data_temp, 2);
	ASS_ReadRegister(ASS_PRESSURE_ADDRESS, data_pres, 2);
	ASS_GetStatus();

	temperature = (data_temp[1] << 8) | data_temp[0];
	pressure = (data_pres[1] << 8) | data_pres[0];

	if(	ass_statusReg.dsp_t_up == 1 || ass_statusReg.dsp_s_up == 1) {
		double calc_steps;
		double result;
		calc_steps = pressure + 26214;
		calc_steps = calc_steps / 52428;
		calc_steps = calc_steps * 1.58;
		calc_steps = calc_steps - 0.79;
		result = round(calc_steps *6894.76 + 4.5); //4.5 is for calibrating to 0
		*data_pressure = (int16_t)result;

		calc_steps = temperature + 32768;
		calc_steps = calc_steps / 65535;
		calc_steps = calc_steps * 105;
		calc_steps = calc_steps - 20;
		calc_steps = calc_steps * 10;
 		*data_temperature = (int16_t)calc_steps;
	}
}

/**
 * @brief Write to selected sensor register
 *
 * description
 *
 * @param address: 		address of register
 * @param byte:			byte to be written to register
*/
void ASS_WriteRegister(uint8_t address, uint8_t byte){
	HAL_I2C_Mem_Write(ass_hi2c, (ASS_SENSOR_ADDRESS << 1) | 0, address, I2C_MEMADD_SIZE_8BIT, &byte, 2, ASS_I2C_MAX_DELAY);
}

/**
 * @brief Read from address for specified length
 *
 * description
 *
 * @param address: 		register address
 * @param pData:		pointer to output data array
 * @param length:		length of data to be read
*/
void ASS_ReadRegister(uint8_t address, uint8_t *pData, uint16_t length){
	HAL_I2C_Mem_Read(ass_hi2c, (ASS_SENSOR_ADDRESS << 1) | 1, address, I2C_MEMADD_SIZE_8BIT, pData, length, ASS_I2C_MAX_DELAY);
}

/**
 * @brief Get status of sensor
 *
 * Reads the sensors status register and stores the information in
 * the ASS_statusReg variable
 *
*/
void ASS_GetStatus(void){
	uint8_t i2c_readData[2];

	ASS_ReadRegister(ASS_STATUS_SYNC_ADDRESS, i2c_readData, 2);

	ass_statusReg.idle           = (i2c_readData[0] >> 0) & 0x01;
	ass_statusReg.reserved1      = (i2c_readData[0] >> 1) & 0x01;
	ass_statusReg.reserved2      = (i2c_readData[0] >> 2) & 0x01;
	ass_statusReg.dsp_s_up       = (i2c_readData[0] >> 3) & 0x01;
	ass_statusReg.dsp_t_up       = (i2c_readData[0] >> 4) & 0x01;
	ass_statusReg.reserved5      = (i2c_readData[0] >> 5) & 0x01;
	ass_statusReg.reserved6      = (i2c_readData[0] >> 6) & 0x01;
	ass_statusReg.bs_fail        = (i2c_readData[0] >> 7) & 0x01;

	ass_statusReg.bc_fail        = (i2c_readData[1] >> 0) & 0x01;
	ass_statusReg.reserved9      = (i2c_readData[1] >> 1) & 0x01;
	ass_statusReg.dsp_sat        = (i2c_readData[1] >> 2) & 0x01;
	ass_statusReg.com_crc_error  = (i2c_readData[1] >> 3) & 0x01;
	ass_statusReg.reserved12     = (i2c_readData[1] >> 4) & 0x01;
	ass_statusReg.reserved13     = (i2c_readData[1] >> 5) & 0x01;
	ass_statusReg.dsp_s_missed   = (i2c_readData[1] >> 6) & 0x01;
	ass_statusReg.dsp_t_missed   = (i2c_readData[1] >> 7) & 0x01;
}

/*
void ASS_SendCAN(uint16_t data_pressure, uint16_t dat_temp) {
	ASS_canData[0] = data_pressure[0] & 0xFF;
	ASS_canData[1] = (data_pressure[1] >> 8) & 0xF;

	ASS_canData[2] = data_temp[0] & 0xFF;
	ASS_canData[3] = (data_temp[1] >> 8) & 0xF;

	ftcan_transmit(0x500, ASS_canData, 4);
}
*/