/*
 * Lepton CCI Module
 *
 * Contains the functions to configure the Lepton via I2C.
 *
 * Copyright 2020-2022 Dan Julio
 *
 * This file is part of tCam.
 *
 * tCam is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * tCam is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with tCam.  If not, see <https://www.gnu.org/licenses/>.
 *
 */
#include "cci.h"
#include "i2c.h"
#include <stdbool.h>
#include <stdio.h>
#include <Arduino.h>
#include "lepton_system.h"

#define CCI_MAX_WAIT_TICKS 5000

//
// CCI Variables
//
static bool cci_last_status_error;
static uint16_t cci_last_status;

// Statically allocated burst read/write I2C buffer sized for up to 512 16-bit words
// plus register starting address
static uint8_t burst_buf[1026];

//
// Forward declarations for primitive access methods
//
static int cci_write_register(uint16_t reg, uint16_t value);
static int cci_write_burst(uint16_t start, uint16_t word_len, uint16_t *buf);
static uint16_t cci_read_register(uint16_t reg);
static int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t *buf);
static uint32_t cci_wait_busy_clear();
static void cci_wait_busy_clear_check(char *cmd);

//
// CCI API
//

/**
 * Write 0 (equivalent to run_cmd) to 512 16-bit words to the lepton and issue
 * the specified command.  Lengths > 16 words are written to the BLOCK data buffer.
 */
void cci_set_reg(uint16_t cmd, int len, uint16_t *buf)
{
	char cmd_buf[11]; // sized for 'cmd 0xNNNN<NULL>'
	int ret = 1;

	cci_last_status_error = false;

	cci_wait_busy_clear();

	if ((len > 0) && (len <= 16))
	{
		ret = cci_write_burst(CCI_REG_DATA_0, len, buf);
	}
	else if ((len > 16) && (len <= 512))
	{
		ret = cci_write_burst(CCI_BLOCK_BUF_0, len, buf);
	}
	else if (len > 512)
	{
		ret = 0;
	}

	if (ret == 1)
	{
		if (len > 0)
		{
			sprintf(cmd_buf, "CMD 0x%4x\n", cmd);
			cci_write_register(CCI_REG_DATA_LENGTH, len);
		}
		else
		{
			sprintf(cmd_buf, "RUN 0x%4x\n", cmd);
		}

		cci_write_register(CCI_REG_COMMAND, cmd);
		cci_wait_busy_clear_check(cmd_buf);
	}
	else
	{
		cci_last_status = 0;
		cci_last_status_error = true;
	}
}

/**
 * Read up to 512 16-bit words form the lepton with the specified command.  Lengths > 16
 * words are read from the BLOCK data buffer.
 */
void cci_get_reg(uint16_t cmd, int len, uint16_t *buf)
{
	char cmd_buf[11]; // sized for 'cmd 0xNNNN<NULL>'

	sprintf(cmd_buf, "CMD 0x%4x", cmd);

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, len);
	cci_write_register(CCI_REG_COMMAND, cmd);
	cci_wait_busy_clear_check(cmd_buf);
	if ((len > 0) && (len <= 16))
	{
		(void)cci_read_burst(CCI_REG_DATA_0, len, buf);
	}
	else if ((len > 16) && (len <= 512))
	{
		(void)cci_read_burst(CCI_BLOCK_BUF_0, len, buf);
	}
}

/**
 * Return true if previous command succeeded as detected by cci_wait_busy_clear_check
 */
bool cci_command_success(uint16_t *status)
{
	*status = cci_last_status;
	return !cci_last_status_error;
}

/**
 * Ping the camera.
 *   Returns 0 for a successful ping
 *   Returns the absolute (postive) 8-bit non-zero LEP_RESULT for a failure
 *   Returns 0x100 (256) for a communications failure
 */
uint32_t cci_run_ping()
{
	uint32_t res;
	uint8_t lep_res;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_RUN_PING);
	res = cci_wait_busy_clear();

	lep_res = (res & 0x000FF00) >> 8; // 8-bit Response Error Code: 0=LEP_OK
	if (res == 0x00010000)
	{
		return 0x100;
	}
	else if (lep_res == 0x00)
	{
		return 0;
	}
	else
	{
		// Convert negative Lepton Response Error Code to a positive number to return
		lep_res = ~lep_res + 1;
		return lep_res;
	}
}

/**
 * Request that a flat field correction occur immediately.
 */
void cci_run_ffc()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_RUN_FFC);
	cci_wait_busy_clear_check("CCI_CMD_SYS_RUN_FFC");
}

/**
 * Get the system uptime.
 */
uint32_t cci_get_uptime()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_UPTIME);
	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_UPTIME");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Get the AUX (case) temperature in Kelvin x 100 (16-bit result).
 */
uint32_t cci_get_aux_temp()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_AUX_TEMP);
	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_AUX_TEMP");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Get the FPA (sensor) temperature in Kelvin x 100 (16-bit result).
 */
uint32_t cci_get_fpa_temp()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_FPA_TEMP);
	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_FPA_TEMP");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Change the telemetry enable state.
 */
void cci_set_telemetry_enable_state(cci_telemetry_enable_state_t state)
{
	uint32_t value = state;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_SET_TELEMETRY_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_SYS_SET_TELEMETRY_ENABLE_STATE");
}

/**
 * Get the telemetry enable state.
 */
uint32_t cci_get_telemetry_enable_state()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_TELEMETRY_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_TELEMETRY_ENABLE_STATE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Change the telemetry location.
 */
void cci_set_telemetry_location(cci_telemetry_location_t location)
{
	uint32_t value = location;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_SET_TELEMETRY_LOCATION);
	cci_wait_busy_clear_check("CCI_CMD_SYS_SET_TELEMETRY_LOCATION");
}

/**
 * Get the telemetry location.
 */
uint32_t cci_get_telemetry_location()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_TELEMETRY_LOCATION);
	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_TELEMETRY_LOCATION");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Get the Gain Mode
 */
void cci_set_gain_mode(cc_gain_mode_t mode)
{
	uint32_t value = mode;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_SET_GAIN_MODE);
	cci_wait_busy_clear_check("CCI_CMD_SYS_SET_GAIN_MODE");
}

/**
 * Set the gain mode
 */
uint32_t cci_get_gain_mode()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_GAIN_MODE);
	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_GAIN_MODE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Change the radiometry enable state.
 */
void cci_set_radiometry_enable_state(cci_radiometry_enable_state_t state)
{
	uint32_t value = state;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_SET_RADIOMETRY_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_ENABLE_STATE");
}

/**
 * Get the radiometry enable state.
 */
uint32_t cci_get_radiometry_enable_state()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_ENABLE_STATE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Set the radiometry flux parameters
 */
void cci_set_radiometry_flux_linear_params(cci_rad_flux_linear_params_t *params)
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, params->sceneEmissivity);
	cci_write_register(CCI_REG_DATA_1, params->TBkgK);
	cci_write_register(CCI_REG_DATA_2, params->tauWindow);
	cci_write_register(CCI_REG_DATA_3, params->TWindowK);
	cci_write_register(CCI_REG_DATA_4, params->tauAtm);
	cci_write_register(CCI_REG_DATA_5, params->TAtmK);
	cci_write_register(CCI_REG_DATA_6, params->reflWindow);
	cci_write_register(CCI_REG_DATA_7, params->TReflK);
	cci_write_register(CCI_REG_DATA_LENGTH, 8);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_SET_RADIOMETRY_FLUX_LINEAR_PARAMS);
	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_FLUX_LINEAR_PARAMS");
}

/**
 * Get the radiometry flux parameters
 */
bool cci_get_radiometry_flux_linear_params(cci_rad_flux_linear_params_t *params)
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 8);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_FLUX_LINEAR_PARAMS);
	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_FLUX_LINEAR_PARAMS");
	params->sceneEmissivity = cci_read_register(CCI_REG_DATA_0);
	params->TBkgK = cci_read_register(CCI_REG_DATA_1);
	params->tauWindow = cci_read_register(CCI_REG_DATA_2);
	params->TWindowK = cci_read_register(CCI_REG_DATA_3);
	params->tauAtm = cci_read_register(CCI_REG_DATA_4);
	params->TAtmK = cci_read_register(CCI_REG_DATA_5);
	params->reflWindow = cci_read_register(CCI_REG_DATA_6);
	params->TReflK = cci_read_register(CCI_REG_DATA_7);

	return !cci_last_status_error;
}

/**
 * Change the radiometry TLinear enable state.
 */
void cci_set_radiometry_tlinear_enable_state(cci_radiometry_tlinear_enable_state_t state)
{
	uint32_t value = state;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_SET_RADIOMETRY_TLINEAR_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_TLINEAR_ENABLE_STATE");
}

/**
 * Get the radiometry TLinear enable state.
 */
uint32_t cci_get_radiometry_tlinear_enable_state()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_ENABLE_STATE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Set the radiometry TLinear Auto Resolution
 */
void cci_set_radiometry_tlinear_auto_res(cci_radiometry_tlinear_auto_res_state_t state)
{
	uint32_t value = state;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_SET_RADIOMETRY_TLINEAR_AUTO_RES);
	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_TLINEAR_AUTO_RES");
}

/**
 * Get the radiometry TLinear Auto Resolution
 */
uint32_t cci_get_radiometry_tlinear_auto_res()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_AUTO_RES);
	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_AUTO_RES");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Set the Radiometry Spotmeter Region-of-interest
 */
void cci_set_radiometry_spotmeter(uint16_t r1, uint16_t c1, uint16_t r2, uint16_t c2)
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, r1);
	cci_write_register(CCI_REG_DATA_1, c1);
	cci_write_register(CCI_REG_DATA_2, r2);
	cci_write_register(CCI_REG_DATA_3, c2);
	cci_write_register(CCI_REG_DATA_LENGTH, 4);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_SET_RADIOMETRY_SPOT_ROI);
	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_SPOT_ROI");
}

/**
 * Get the Radiometry Spotmeter Region-of-interest
 */
bool cci_get_radiometry_spotmeter(uint16_t *r1, uint16_t *c1, uint16_t *r2, uint16_t *c2)
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 4);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_SPOT_ROI);
	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_SPOT_ROI");
	*r1 = cci_read_register(CCI_REG_DATA_0);
	*c1 = cci_read_register(CCI_REG_DATA_1);
	*r2 = cci_read_register(CCI_REG_DATA_2);
	*c2 = cci_read_register(CCI_REG_DATA_3);

	return !cci_last_status_error;
}

/**
 * Get the AGC enable state.
 */
uint32_t cci_get_agc_enable_state()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_AGC_GET_AGC_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_AGC_GET_AGC_ENABLE_STATE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Set the AGC enable state.
 */
void cci_set_agc_enable_state(cci_agc_enable_state_t state)
{
	uint32_t value = state;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_AGC_SET_AGC_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_AGC_SET_AGC_ENABLE_STATE");
}

/**
 * Get the AGC calc enable state.
 */
uint32_t cci_get_agc_calc_enable_state()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_AGC_GET_CALC_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_AGC_GET_CALC_ENABLE_STATE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Set the AGC calc enable state.
 */
void cci_set_agc_calc_enable_state(cci_agc_enable_state_t state)
{
	uint32_t value = state;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_AGC_SET_CALC_ENABLE_STATE);
	cci_wait_busy_clear_check("CCI_CMD_AGC_SET_CALC_ENABLE_STATE");
}

/**
 * Run the Reboot command
 */
void cc_run_oem_reboot()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_OEM_RUN_REBOOT);
	// Sleep to allow camera to reboot and run FFC
	delay(6000);
	cci_wait_busy_clear_check("CCI_CMD_OEM_RUN_REBOOT");
}

/**
 * Get the GPIO mode.
 */
uint32_t cci_get_gpio_mode()
{
	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_OEM_GET_GPIO_MODE);
	cci_wait_busy_clear_check("CCI_CMD_OEM_GET_GPIO_MODE");

	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
	return ms_word << 16 | ls_word;
}

/**
 * Set the GPIO mode.
 */
void cci_set_gpio_mode(cci_gpio_mode_t mode)
{
	uint32_t value = mode;

	cci_wait_busy_clear();
	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
	cci_write_register(CCI_REG_DATA_LENGTH, 2);
	cci_write_register(CCI_REG_COMMAND, CCI_CMD_OEM_SET_GPIO_MODE);
	cci_wait_busy_clear_check("CCI_CMD_OEM_SET_GPIO_MODE");
}

/**
 * Get the FLIR systems part number
 *   - call with a 32-character buffer
 */
void cci_get_part_number(char *pn)
{
	bool low_half = true;
	int i = 0;
	uint16_t cci_buf[16];

	int t = 0; // maximum tick count

	cci_get_reg(CCI_CMD_OEM_GET_PART_NUM, 16, cci_buf);

	*pn = (char)(cci_buf[0] & 0xFF);
	while ((*pn != 0) && (i < 16) && (t++ < CCI_MAX_WAIT_TICKS))
	{
		low_half = !low_half;
		if (low_half)
		{
			*(++pn) = (char)(cci_buf[i] & 0xFF);
		}
		else
		{
			*(++pn) = (char)(cci_buf[i] >> 8);
			i++;
		}
	}
	*(++pn) = 0;
}

#define TAG_CCI "[CCI]"

//
// Primitive access methods
//

/**
 * Write a CCI register.
 */
static int cci_write_register(uint16_t reg, uint16_t value)
{
	// Write the register address and value
	uint8_t write_buf[4] = {
		reg >> 8 & 0xff,
		reg & 0xff,
		value >> 8 & 0xff,
		value & 0xff};
	esp_err_t ret = i2c_master_transmit(LEPTON_DEV_HANDLE, write_buf, sizeof(write_buf), I2C_TIMEOUT_MS_VALUE);
	if (ret == ESP_OK)
	{
		ESP_LOGV(TAG_CCI, "Write to register 0x%04X successful (Value: 0x%04X)", reg, value);
		return 1;
	}
	else
	{
		ESP_LOGE(TAG_CCI, "Write to register 0x%04X failed: %s", reg, esp_err_to_name(ret));
		return -1;
	}
}

/**
 * Burst write a group of CCI data registers
 */
static int cci_write_burst(uint16_t start, uint16_t word_len, uint16_t *buf)
{
	int i;

	size_t bufSize = sizeof(*buf) * (1 + word_len);
	unsigned char *buffer = (unsigned char *)malloc(bufSize);

	// Create the i2c transaction buffer
	buffer[0] = start >> 8;
	buffer[1] = start & 0xFF;
	for (i = 1; i <= word_len; i++)
	{
		buffer[i * 2] = *buf >> 8;
		buffer[i * 2 + 1] = *buf++ & 0xFF;
	}

	esp_err_t ret = i2c_master_transmit(LEPTON_DEV_HANDLE, buffer, bufSize, I2C_TIMEOUT_MS_VALUE);
	free(buffer);
	if (ret == ESP_OK)
	{
		ESP_LOGV(TAG_CCI, "Burst write to register 0x%04X successful", start);
		return 1;
	}
	else
	{
		ESP_LOGE(TAG_CCI, "Burst write to register 0x%04X failed: %s", start, esp_err_to_name(ret));
		return -1;
	}
}

/**
 * Read a CCI register.
 */

/**
 * @brief Reads a 16-bit value from a specific register of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param reg_addr Register address to read from
 * @return value The register value
 */
uint16_t cci_read_register(uint16_t reg_addr)
{
	uint8_t buffer[2];

	esp_err_t ret = i2c_master_transmit_receive(LEPTON_DEV_HANDLE, (uint8_t *)&reg_addr, sizeof(reg_addr), buffer, sizeof(buffer), I2C_TIMEOUT_MS_VALUE);
	if (ret == ESP_OK)
	{
		uint16_t value = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF);
		ESP_LOGV(TAG_CCI, "Read from register 0x%04X successful (Value: 0x%04X)", reg_addr, value);
		return value;
	}
	else
	{
		ESP_LOGE(TAG_CCI, "Read from register 0x%04X failed: %s", reg_addr, esp_err_to_name(ret));
		return -1;
	}
}

/**
 * @brief Reads a burst of 16-bit values from specific registers of an I2C device.
 *
 * @param dev_handle I2C device handle
 * @param start Register address to read from
 * @param word_len Register address to read from
 * @return esp_err_t ESP_OK on success, or an error code
 */
int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t *buf)
{

	if (!buf)
	{
		return -1;
	}
	size_t bufSize = word_len * sizeof(*buf);
	unsigned char *buffer = (unsigned char *)malloc(bufSize);
	esp_err_t ret = i2c_master_transmit_receive(LEPTON_DEV_HANDLE, (uint8_t *)&start, sizeof(start), buffer, bufSize, I2C_TIMEOUT_MS_VALUE);
	if (ret == ESP_OK)
	{
		ESP_LOGV(TAG_CCI, "Read from register 0x%04X successful", start);
		for (int i = 0; i < word_len; i++)
		{ // CHECK IF THIS IS NEEDED
			*buf++ = buffer[i * 2] << 8 | buffer[i * 2 + 1];
		}
		free(buffer);
		return 1;
	}
	else
	{
		ESP_LOGE(TAG_CCI, "Read from register 0x%04X failed: %s", start, esp_err_to_name(ret));
		return -1;
	}
}

/**
 * Wait for busy to be clear in the status register
 *   Returns the 16-bit STATUS
 *   Returns 0x00010000 if there is a communication failure
 */
/**
 * Wait for busy to be clear in the status register
 *   Returns the 16-bit STATUS
 *   Returns 0x00010000 if there is a communication failure
 */
static uint32_t cci_wait_busy_clear()
{
	bool err = false;
	uint8_t tx_buf[2]; // Register address to read from
	uint8_t rx_buf[2]; // Value buffer

	uint16_t reg_addr = 0x0002; // STATUS register
	int t = 0;

	while (!err)
	{
		if (t++ >= CCI_MAX_WAIT_TICKS)
		{
			err = true;
			break;
		}

		// Prepare register address in big-endian
		tx_buf[0] = (reg_addr >> 8) & 0xFF;
		tx_buf[1] = reg_addr & 0xFF;

		esp_err_t ret = i2c_master_transmit_receive(LEPTON_DEV_HANDLE, tx_buf, sizeof(tx_buf), rx_buf, sizeof(rx_buf), I2C_TIMEOUT_MS_VALUE);
		if (ret != ESP_OK)
		{
			ESP_LOGE(TAG_CCI, "Failed to read STATUS register: %s", esp_err_to_name(ret));
			return 0x00010000;
		}

		uint16_t status = ((uint16_t)rx_buf[0] << 8) | rx_buf[1];

		ESP_LOGV(TAG_CCI, "STATUS register read: 0x%04X", status);

		// Check bits [2:0] == 0b110 (0x06)
		if ((rx_buf[1] & 0x07) == 0x06)
		{
			return status;
		}
	}

	// Timed out
	return 0x00010000;
}


/**
 * Wait for busy to be clear in the status register and check the result
 * printing an error if detected
 */
static void cci_wait_busy_clear_check(char *cmd)
{
	int8_t response;
	uint32_t t32;

	cci_last_status_error = false;

	t32 = cci_wait_busy_clear();
	cci_last_status = t32 & 0xFFFF;
	if (t32 == 0x00010000)
	{
		printf("[CCI] Error: cmd: %s failed wait_busy_clear\n", cmd);
		cci_last_status_error = true;
	}
	else
	{
		response = (int8_t)((t32 & 0x0000FF00) >> 8);
		if (response < 0)
		{
			printf("[CCI] Error: %s returned %d\n", cmd, response);
		}
	}
}