/**
 * @file HTPA_32x32d.c
 * @brief Library for HTPA 32x32d infrared array sensor
 * @author Tim-Erik Düntzsch t.duentzsch@fasttube.de
 * 
 * @date 08.03.2024 - successful readout of block 3 top half and conversion factors
 * 
 * @test eeprom readout and temperature conversion
 * 
 * @version 0.7
 */

#include <stdbool.h>
#include <math.h>
#include "main.h"
#include "HTPA_32x32d.h"
#include "HTPA_lookuptable.h"

// I2C address
#define HTPA_SENSOR_ADDRESS		0x1A
#define HTPA_EEPROM_ADDRESS		0x50

// Sensor configuration registers (write only)
#define HTPA_SENSOR_CONFIG		0x01	// Configuration register
#define HTPA_SENSOR_TRIM_1		0x03	// Amplification and ADC resolution
#define HTPA_SENSOR_TRIM_2		0x04	// Bias current of Top ADC
#define HTPA_SENSOR_TRIM_3		0x05	// Bias current of Bot ADC
#define HTPA_SENSOR_TRIM_4		0x06	// Clock frequency
#define HTPA_SENSOR_TRIM_5		0x07	// Common mode voltage preamplifier top
#define HTPA_SENSOR_TRIM_6		0x08	// Common mode voltage preamplifier bot
#define HTPA_SENSOR_TRIM_7		0x09	// Internal pull-ups SDA, SCL
// Sensor read only registers
#define HTPA_SENSOR_STATUS		0x02	// Status register
#define HTPA_SENSOR_READTOP		0x0A	// Read top half
#define HTPA_SENSOR_READBOT		0x0B	// Read bot half
// EEPROM addresses
#define HTPA_EEPROM_VDDCOMPGRAD	0x0340	// Start address for vddcompgrad[i][j]
#define HTPA_EEPROM_VDDCOMPOFF	0x0540	// Start address for vddcompoff[i][j]
#define HTPA_EEPROM_THGRAD		0x0A40	// Start address for thgrad[i][j] (top, block3, pixel 384 -> 0x0740 + 2*384 = 0x0A40
#define HTPA_EEPROM_THOFFSET	0x1240	// Start address for thoffset[i][j] (top, block3, pixel 384 -> 0x0F40 + 2*384 = 0x1240
#define HTPA_EEPROM_PI			0x1A40	// Start address for pij[i][j] (top, block3, pixel 384 -> 0x1740 + 2*384 = 0x1A40




// I2C transmit delay
#define HTPA_I2C_MAX_DELAY			0xFF

I2C_HandleTypeDef* htpa_hi2c;	// pointer to i2c handle

// EEPROM data:
uint8_t gradscale, vddscgrad, vddscoff, epsilon, arraytype, nrofdefpix;
int8_t globaloff;
uint16_t vddth1, vddth2, ptatth1, ptatth2, globalgain, tablenumber;
uint16_t pij[4][32];
int16_t thgrad[4][32];
int16_t thoffset[4][32];
int16_t vddcompgrad[4][32];
int16_t vddcompoff[4][32];
float pixcmin, pixcmax, ptatgr, ptatoff;

// Sensor data:
HTPA_Status htpa_statusReg;
uint8_t data_topBlock[258];
uint8_t elOffset_topBlock[258];
uint16_t vdd_topBlock, ptat_topBlock;
uint16_t pixel_topBlock[4][32];
uint16_t elOffset[4][32];

// Calculated values:
uint32_t gradscale_div, vddscgrad_div, vddscoff_div;
int32_t pixcij[4][32];			// sensitivity coefficients per pixel (needed for 11.5)
int32_t vij_comp[4][32];		// thermal offset compensated (11.2)
int32_t vij_comp_s[4][32];		// electrical offset compensated (11.3)
int32_t vij_vddcomp[4][32];		// vdd compensated (11.4)
int32_t vij_pixc[4][32];		// sensitivity coefficients applied (11.5)
uint32_t temp_pix[4][32];		// final pixel temperature in dK (11.7)
float ambient_temperature;

/**
 * @brief Initialization of HTPA Sensor
 * 
 * Sets the wakeup bit in the status register and writes the desired sensor
 * configuration to the respective registers.
 * Afterwards the sensor is in idle and ready for conversion.
 * 
 * @param *hi2c: Pointer to I2C Handle
*/
void HTPA_Init(I2C_HandleTypeDef *hi2c){
	htpa_hi2c = hi2c;
	// I2C initialized on 400kbit Fast Mode
	/*
	 * Read EEPROM calibration values
	 * (see datasheet Figure 13)
	*/
	uint8_t eeprom_float[4] = {0};
	eeprom_float[0] = HTPA_ReadEEPROM_byte(0x0000);
	eeprom_float[1] = HTPA_ReadEEPROM_byte(0x0001);
	eeprom_float[2] = HTPA_ReadEEPROM_byte(0x0002);
	eeprom_float[3] = HTPA_ReadEEPROM_byte(0x0003);
	pixcmin = *(float*)eeprom_float;
	eeprom_float[0] = HTPA_ReadEEPROM_byte(0x0004);
	eeprom_float[1] = HTPA_ReadEEPROM_byte(0x0005);
	eeprom_float[2] = HTPA_ReadEEPROM_byte(0x0006);
	eeprom_float[3] = HTPA_ReadEEPROM_byte(0x0007);
	pixcmax = *(float*)eeprom_float;
	gradscale = HTPA_ReadEEPROM_byte(0x0008);
	tablenumber = HTPA_ReadEEPROM_byte(0x000C) << 8 | HTPA_ReadEEPROM_byte(0x000B);
	epsilon = HTPA_ReadEEPROM_byte(0x000D);
	arraytype = HTPA_ReadEEPROM_byte(0x0022);
	vddth1 = HTPA_ReadEEPROM_byte(0x0027) << 8 | HTPA_ReadEEPROM_byte(0x0026);
	vddth2 = HTPA_ReadEEPROM_byte(0x0029) << 8 | HTPA_ReadEEPROM_byte(0x0028);
	eeprom_float[0] = HTPA_ReadEEPROM_byte(0x0034);
	eeprom_float[1] = HTPA_ReadEEPROM_byte(0x0035);
	eeprom_float[2] = HTPA_ReadEEPROM_byte(0x0036);
	eeprom_float[3] = HTPA_ReadEEPROM_byte(0x0037);
	ptatgr = *(float*)eeprom_float;
	eeprom_float[0] = HTPA_ReadEEPROM_byte(0x0038);
	eeprom_float[1] = HTPA_ReadEEPROM_byte(0x0039);
	eeprom_float[2] = HTPA_ReadEEPROM_byte(0x003A);
	eeprom_float[3] = HTPA_ReadEEPROM_byte(0x003B);
	ptatoff = *(float*)eeprom_float;
	ptatth1 = HTPA_ReadEEPROM_byte(0x003D) << 8 | HTPA_ReadEEPROM_byte(0x003C);
	ptatth2 = HTPA_ReadEEPROM_byte(0x003F) << 8 | HTPA_ReadEEPROM_byte(0x003E);
	vddscgrad = HTPA_ReadEEPROM_byte(0x004E);
	vddscoff = HTPA_ReadEEPROM_byte(0x004F);
	globaloff = HTPA_ReadEEPROM_byte(0x0054);
	globalgain = HTPA_ReadEEPROM_byte(0x0056) << 8 | HTPA_ReadEEPROM_byte(0x0055);
	nrofdefpix = HTPA_ReadEEPROM_byte(0x007F);
	for(uint8_t i = 0; i < 32; i++) {
		vddcompgrad[0][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 2 * i);
		vddcompgrad[1][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 1 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 1 * 64 + 2 * i);
		vddcompgrad[2][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 2 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 2 * 64 + 2 * i);
		vddcompgrad[3][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 3 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPGRAD + 3 * 64 + 2 * i);
		// ignore bottom half
	}
	for(uint8_t i = 0; i < 32; i++) {
		vddcompoff[0][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 2 * i);
		vddcompoff[1][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 1 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 1 * 64 + 2 * i);
		vddcompoff[2][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 2 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 2 * 64 + 2 * i);
		vddcompoff[3][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 3 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_VDDCOMPOFF + 3 * 64 + 2 * i);
		// ignore bottom half
	}
	for(uint8_t i = 0; i < 32; i++) {
		// start at block 3 (pixel 384)
		thgrad[0][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 2 * i);
		thgrad[1][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 1 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 1 * 64 + 2 * i);
		thgrad[2][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 2 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 2 * 64 + 2 * i);
		thgrad[3][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 3 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THGRAD + 3 * 64 + 2 * i);
		// ignore bottom half
	}
	for(uint8_t i = 0; i < 32; i++) {
		// start at block 3 (pixel 384)
		thoffset[0][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 2 * i);
		thoffset[1][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 1 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 1 * 64 + 2 * i);
		thoffset[2][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 2 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 2 * 64 + 2 * i);
		thoffset[3][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 3 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_THOFFSET + 3 * 64 + 2 * i);
		// ignore bottom half
	}
	for(uint8_t i = 0; i < 32; i++) {
		// start at block 3 (pixel 384)
		pij[0][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 2 * i);
		pij[1][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 1 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 1 * 64 + 2 * i);
		pij[2][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 2 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 2 * 64 + 2 * i);
		pij[3][i] = HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 3 * 64 + 2 * i + 1) << 8 | HTPA_ReadEEPROM_byte(HTPA_EEPROM_PI + 3 * 64 + 2 * i);
		// ignore bottom half
	}


	/* Set I2C to Fast Mode Plus (1Mbit) for sensor readout: */
	if (HAL_I2C_DeInit(htpa_hi2c) != HAL_OK)
	{
		Error_Handler();
	}
	htpa_hi2c->Init.Timing = 0x00000107;
	if (HAL_I2C_Init(htpa_hi2c) != HAL_OK)
	{
		Error_Handler();
	}
	__HAL_SYSCFG_FASTMODEPLUS_ENABLE(I2C_FASTMODEPLUS_I2C1);
	HAL_Delay(100);


	/*
	 * Write sensor calibration registers
	*/
	HTPA_WriteRegister(HTPA_SENSOR_CONFIG, 0x01);	// wakeup
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_1, 0x0C);	// bit 5,4 = 00 -> amplification = 0, bit 3-0 = 1100 -> 16bit ADC-Resolution (4 + m=12)
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_2, 0x0C);
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_3, 0x0C);
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_4, 0x14);	// clock frequency set to 0x14 -> 4.75MHz -> time for quarter frame: ~27ms
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_5, 0x0C);
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_6, 0x0C);
	HAL_Delay(10);
	HTPA_WriteRegister(HTPA_SENSOR_TRIM_7, 0x88);
	HAL_Delay(10);
	//HTPA_WriteRegister(HTPA_SENSOR_CONFIG, 0x09);	// start sensor
	//HAL_Delay(10);

	/*
	 * Calculations
	 */
	gradscale_div = HTPA_calcPowerTwo(gradscale);
	vddscgrad_div = HTPA_calcPowerTwo(vddscgrad);
	vddscoff_div = HTPA_calcPowerTwo(vddscoff);
	// calculate sensitivity coefficients: (datasheet 11.5)
	for(uint8_t i = 0; i < 4; i++) {
		for(uint8_t j = 0; j < 32; j++) {
			pixcij[i][j] = (int32_t)pixcmax - (int32_t)pixcmin;
			pixcij[i][j] = pixcij[i][j] / 65535;
			pixcij[i][j] = pixcij[i][j] * pij[i][j];
			pixcij[i][j] = pixcij[i][j] + pixcmin;
			pixcij[i][j] = pixcij[i][j] * 1.0 * epsilon / 100;
			pixcij[i][j] = pixcij[i][j] * 1.0 * globalgain / 1000;
		}
	}
}

uint32_t HTPA_calcPowerTwo(uint8_t power) {
	uint32_t powerTwo = power;
	return powerTwo;
}

void HTPA_ReadSensor(void) {
	uint8_t config = 0;
	/*
	 * Read top array half of block3 with PTAT
	 */
	// write block and vdd/ptat selection to config register:
	config |= (3 << 4);				// bit 5,4 block 3 selection
	config |= 0x09;					// bit 3 start | bit 0 wakeup
	HTPA_WriteRegister(HTPA_SENSOR_CONFIG, config);
	HAL_Delay(30); 					// conversion around 27ms in standard config
	HTPA_GetStatus();
	while(htpa_statusReg.eoc != 1) {
		HAL_Delay(1);
		HTPA_GetStatus();
	} // wait until eoc flag is set then read register data
	HTPA_ReadRegister(HTPA_SENSOR_READTOP, data_topBlock, 258);

	ptat_topBlock = (data_topBlock[0] << 8) | data_topBlock[1];

	/*
	 * Read electrical offset with VDD
	 */
	config |= 0x04;			// bit 2 vdd_meas
	config |= 0x02;			// bit 1 blind for electrical offset readout (block selection is ignored)
	HTPA_WriteRegister(HTPA_SENSOR_CONFIG, config);
	HAL_Delay(30); 					// conversion around 27ms in standard config
	while(htpa_statusReg.eoc != 1) {
		HAL_Delay(1);
		HTPA_GetStatus();
	} // wait until eoc flag is set then read register data
	HTPA_ReadRegister(HTPA_SENSOR_READTOP, elOffset_topBlock, 258);

	vdd_topBlock = (elOffset_topBlock[0] << 8) | elOffset_topBlock[1];

	/*
	 * Sort sensor data and assign to pixels
	 */
	for(int i=0; i<32; i++) {
		pixel_topBlock[0][i] = (data_topBlock[2*i + 2] << 8) | data_topBlock[2*i + 3];
		pixel_topBlock[1][i] = (data_topBlock[2*(i+32) + 2] << 8) | data_topBlock[2*(i+32) + 3];
		pixel_topBlock[2][i] = (data_topBlock[2*(i+64) + 2] << 8) | data_topBlock[2*(i+64) + 3];
		pixel_topBlock[3][i] = (data_topBlock[2*(i+96) + 2] << 8) | data_topBlock[2*(i+96) + 3];

		elOffset[0][i] = (elOffset_topBlock[2*i + 2] << 8) | elOffset_topBlock[2*i + 3];
		elOffset[1][i] = (elOffset_topBlock[2*(i+32) + 2] << 8) | elOffset_topBlock[2*(i+32) + 3];
		elOffset[2][i] = (elOffset_topBlock[2*(i+64) + 2] << 8) | elOffset_topBlock[2*(i+64) + 3];
		elOffset[3][i] = (elOffset_topBlock[2*(i+96) + 2] << 8) | elOffset_topBlock[2*(i+96) + 3];
	}


	/*
	 * calculate temperature
	 */
	int64_t vij_pixc_and_pcscaleval;
	int64_t vdd_calc_steps;
	uint16_t table_row, table_col;
	int32_t vx, vy, ydist, dta;

	// 11.1 ambient temperature:
	ambient_temperature = ptat_topBlock * ptatgr + ptatoff;		// value in dK

	// find column of lookup table (ambient temperature)
	for(uint8_t i = 0; i < NROFTAELEMENTS; i++) {
		if(ambient_temperature > XTATemps[i]) {
			table_col = i;
		}
	}
	dta = ambient_temperature - XTATemps[table_col];
	ydist = (int32_t)ADEQUIDISTANCE;

	for(int i=0; i<4; i++) {
		for(int j=0; j<32; j++) {
			// 11.2 thermal offset:
			vij_comp[i][j] = pixel_topBlock[i][j] - (thgrad[i][j] * ptat_topBlock / gradscale_div) - thoffset[i][j];

			// 11.3 electrical offset:
			vij_comp_s[i][j] = vij_comp[i][j] - elOffset[i % 4][j];

			// 11.4 Vdd compensation:
			vdd_calc_steps = vddcompgrad[i%4][j] * ptat_topBlock;
			vdd_calc_steps = vdd_calc_steps / vddscgrad_div;
			vdd_calc_steps = vdd_calc_steps + vddcompoff[i%4][j];
			vdd_calc_steps = vdd_calc_steps * (vdd_topBlock - vddth1 - ((vddth2 - vddth1) / (ptatth1 - ptatth2)) * (ptat_topBlock - ptatth1));
			vdd_calc_steps = vdd_calc_steps / vddscoff_div;
			vij_vddcomp[i][j] = vij_comp_s[i][j] - vdd_calc_steps;

			// 11.5 calculate object temperature
			vij_pixc_and_pcscaleval = (int64_t)vij_vddcomp[i][j] * (int64_t)PCSCALEVAL;
			vij_pixc[i][j] = (int32_t)(vij_pixc_and_pcscaleval / (int64_t)pixcij[i][j]);
			// find temperature in lookup table and do bilinear interpolation
			/*
			table_row = vij_pixc[i][j] + TABLEOFFSET;
			table_row = table_row >> ADEXPBITS;
			vx = ((((int32_t)TempTable[table_row][table_col + 1] - (int32_t)TempTable[table_row][table_col]) * (int32_t)dta) / (int32_t)TAEQUIDISTANCE) + (int32_t)TempTable[table_row][table_col];
			vy = ((((int32_t)TempTable[table_row + 1][table_col + 1] - (int32_t)TempTable[table_row + 1][table_col]) * (int32_t)dta) / (int32_t)TAEQUIDISTANCE) + (int32_t)TempTable[table_row + 1][table_col];
			temp_pix[i][j] = (uint32_t)((vy - vx) * ((int32_t)(vij_pixc[i][j] + TABLEOFFSET) - (int32_t)YADValues[table_row]) / ydist + (int32_t)vx);

			// --- GLOBAL OFFSET ---
			temp_pix[i][j] = temp_pix[i][j] + globaloff;
			*/
		}
	}

}

/**
 * @brief Write to selected sensor register
 * 
 * description
 * 
 * @param address: 		address of register
 * @param byte:			byte to be written to register
*/
void HTPA_WriteRegister(uint8_t address, uint8_t byte){
	HAL_I2C_Mem_Write(htpa_hi2c, (HTPA_SENSOR_ADDRESS << 1), address, I2C_MEMADD_SIZE_8BIT, &byte, 1, HTPA_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 HTPA_ReadRegister(uint8_t address, uint8_t *pData, uint16_t length){
	HAL_I2C_Mem_Read(htpa_hi2c, (HTPA_SENSOR_ADDRESS << 1), address, I2C_MEMADD_SIZE_8BIT, pData, length, HTPA_I2C_MAX_DELAY);
}

/**
 * @brief Get status of sensor
 * 
 * Reads the sensors status register and stores the information in
 * the htpa_statusReg variable
 * 
*/
void HTPA_GetStatus(void){
	uint8_t i2c_readData = 0;

	HTPA_ReadRegister(HTPA_SENSOR_STATUS, &i2c_readData, 1);

	htpa_statusReg.block = (i2c_readData >> 4) & 0x03;
	htpa_statusReg.vdd_meas = (i2c_readData >> 2) & 0x01;
	htpa_statusReg.blind = (i2c_readData >> 1) & 0x01;
	htpa_statusReg.eoc = i2c_readData & 0x01;
}

/**
 * @brief Get status of sensor
 *
 * Reads the sensors status register and stores the information in
 * the htpa_statusReg variable
 *
*/
uint8_t HTPA_ReadEEPROM_byte(uint16_t address){
	uint8_t data = 0;
	HAL_I2C_Mem_Read(htpa_hi2c, (HTPA_EEPROM_ADDRESS << 1), address, I2C_MEMADD_SIZE_16BIT, &data, 1, HTPA_I2C_MAX_DELAY);
	return data;
}