/**
 * @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"

// 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	// Interal 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
// I2C transmit delay
#define HTPA_I2C_MAX_DELAY			0xFFFFFFFF

I2C_HandleTypeDef* htpa_hi2c;	// pointer to i2c handle

HTPA_Status htpa_statusReg;

uint8_t data_topBlock[258];
uint8_t elOffset_topBlock[258];
//uint8_t data_botBlock[258];

uint16_t vdd_topBlock;
uint16_t ptat_topBlock;

uint16_t pixel_topBlock[4][32];

/**
 * @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;
	/*
	 * Read EEPROM calibration values
	 *
	HAL_I2C_DeInit(htpa_hi2c);
	htpa_hi2c->Init->Timing = 0;		// set I2C frequency to 400kHz
	HAL_I2C_Init(htpa_hi2c);
	EEPROM auslesen:
	HTPA_ReadEEPROM(
	*/


	/*
	 * Write sensor calibration registers
	 *
	HAL_I2C_DeInit(htpa_hi2c);
	htpa_hi2c->Init->Timing = 0;		// set I2C frequency to 1MHz
	HAL_I2C_Init(htpa_hi2c);
	*/

	// Berechnung für clk / sample aus I2C parametern?
	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);
}

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];
	}

	/*
	 * calculate temperature
	 *
	// 11.1 ambient temperature:
	float t_ambient = ptat_topBlock*ptat_gradient + ptat_offset;
	for(int i=0; i<4; i++) {
		for(int j=0; j<32; j++) {
			// 11.2 thermal offset:
			vij_comp[i][j] = vij[i][j] - (thGrad[i][j]*ptat_topBlock/pow(2, gradScale)) - thOffset[i][j];
			// 11.3 electrical offset:
			vij_compElec[i][j] = vij[i][j] - elOffset[(i+j*32)%128];
			// 11.4 Vdd compensation:
			vij_compVdd[i][j] = vij_compElec[i][j] * ...
			// 11.5 calculate object temperature
		}
	}
	*/
}

/**
 * @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
 * @return
*/
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;
}