/*
 * Lepton related utilities
 *
 * Contains utility and access functions for the Lepton.
 *
 * Note: I noticed that on occasion, the first time some commands run on the lepton
 * will fail either silently or with an error response code.  The access routines in
 * this module attempt to detect and retry the commands if necessary.
 *
 * Copyright 2020 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 "lepton_utilities.h"
#include "cci.h"
#include "i2c.h"
#include "esp_system.h"
#include "vospi.h"
#include <string.h>
#include <Arduino.h>


static bool lep_is_radiometric = false;
static int lep_type;

// default lepton config
lepton_config_t LEP_CONFIG = {
	.agc_set_enabled = false,
	.emissivity = 100,
	.gain_mode = LEP_GAIN_HIGH
};

//
// Lepton Utilities API
//
bool lepton_init()
{
	char pn[33];
	uint32_t val, rsp;
	lepton_config_t* lep_stP = &LEP_CONFIG;

  // Attempt to ping the Lepton to validate communication
  // If this is successful, we assume further communication will be successful
  rsp = cci_run_ping();
  if (rsp != 0) {
    printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
    return false;
	}
	
	// Get the Lepton type (part number) to determine if it's a Lepton 3.0 or 3.5
	cci_get_part_number(pn);
	printf("[LEP UTIL] Found Lepton, part number: %s\n", pn);
	if (strncmp(pn, "500-0771-01", 32) == 0) {
		printf("  Radiometric Lepton 3.5\n");
		lep_is_radiometric = true;
		lep_type = LEP_TYPE_3_5;
	} else if (strncmp(pn, "500-0758-99", 32) == 0) {
		printf("  Radiometric Lepton 3.1\n");
		lep_is_radiometric = true;
		lep_type = LEP_TYPE_3_1;
	} else if (strncmp(pn, "500-0726-01", 32) == 0) {
		printf("  Non-radiometric Lepton 3.0\n");
		lep_is_radiometric = false;
		lep_type = LEP_TYPE_3_0;
	} else {
		printf("  Unsupported Lepton");
		lep_is_radiometric = true;
		lep_type = LEP_TYPE_UNK;
	}
	
	if (lep_is_radiometric) {
		// Configure Radiometry for TLinear enabled, auto-resolution
		cci_set_radiometry_enable_state(CCI_RADIOMETRY_ENABLED);
		rsp = cci_get_radiometry_enable_state();
		printf("[LEP UTIL] Lepton Radiometry = %d\n", rsp);
		if (rsp != CCI_RADIOMETRY_ENABLED) {
			// Make one more effort
			delay(10);
			printf("[LEP UTIL] Retry Set Lepton Radiometry\n");
			cci_set_radiometry_enable_state(CCI_RADIOMETRY_ENABLED);
			rsp = cci_get_radiometry_enable_state();
			printf("[LEP UTIL] Lepton Radiometry = %d\n", rsp);
			if (rsp != CCI_RADIOMETRY_ENABLED) {
				printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
				return false;
			}
		}
	
		// TLinear depends on AGC
		val = (lep_stP->agc_set_enabled) ? CCI_RADIOMETRY_TLINEAR_DISABLED : CCI_RADIOMETRY_TLINEAR_ENABLED;
		cci_set_radiometry_tlinear_enable_state((cci_radiometry_tlinear_enable_state_t)val);
		rsp = cci_get_radiometry_tlinear_enable_state();
		printf("[LEP UTIL] Lepton Radiometry TLinear = %d\n", rsp);
		if (rsp != val) {
			printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
	  		return false;
		}
		
		cci_set_radiometry_tlinear_auto_res(CCI_RADIOMETRY_AUTO_RES_ENABLED);
		rsp = cci_get_radiometry_tlinear_auto_res();
		printf("[LEP UTIL] Lepton Radiometry Auto Resolution = %d\n", rsp);
		if (rsp != CCI_RADIOMETRY_AUTO_RES_ENABLED) {
			printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
	  		return false;
		}
	}
	
	// Enable AGC calcs for a smooth transition between modes
	cci_set_agc_calc_enable_state(CCI_AGC_ENABLED);
	rsp = cci_get_agc_calc_enable_state();
	printf("[LEP UTIL] Lepton AGC Calcs = %d\n", rsp);
	if (rsp != CCI_AGC_ENABLED) {
		printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
  		return false;
	}
	
	// AGC
	val = (lep_stP->agc_set_enabled) ? CCI_AGC_ENABLED : CCI_AGC_DISABLED;
	cci_set_agc_enable_state((cci_agc_enable_state_t)val);
	rsp = cci_get_agc_enable_state();
	printf("[LEP UTIL] Lepton AGC = %d\n", rsp);
	if (rsp != val) {
		printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
  		return false;
	}
	
	// Enable telemetry
	cci_set_telemetry_enable_state(CCI_TELEMETRY_ENABLED);
	rsp = cci_get_telemetry_enable_state();
	printf("[LEP UTIL] Lepton Telemetry = %d\n", rsp);
	if (rsp != CCI_TELEMETRY_ENABLED) {
		printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
  		return false;
	}
	vospi_include_telem(true);
	
	// GAIN
	switch (lep_stP->gain_mode) {
		case LEP_GAIN_HIGH:
			val = LEP_SYS_GAIN_MODE_HIGH;
			break;
		case LEP_GAIN_LOW:
			val = LEP_SYS_GAIN_MODE_LOW;
			break;
		default:
			val = LEP_SYS_GAIN_MODE_AUTO;
	}
	cci_set_gain_mode((cc_gain_mode_t)val);
	rsp = cci_get_gain_mode();
	printf("[LEP UTIL] Lepton Gain Mode = %d\n", rsp);
	if (rsp != val) {
		printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
  		return false;
	}
	
	// Emissivity
	if (lep_is_radiometric) {
		lepton_emissivity(lep_stP->emissivity);
		printf("[LEP UTIL] Lepton Emissivity = %d%%\n", lep_stP->emissivity);
	}
  	
	// Finally enable VSYNC on Lepton GPIO3
	cci_set_gpio_mode(LEP_OEM_GPIO_MODE_VSYNC);
	rsp = cci_get_gpio_mode();
	printf("[LEP UTIL] Lepton GPIO Mode = %d\n", rsp);
	if (rsp != LEP_OEM_GPIO_MODE_VSYNC) {
		printf("[LEP UTIL] Error: Lepton communication failed (%d)\n", rsp);
  		return false;
	}
	
	return true;
}


bool lepton_is_radiometric()
{
	return lep_is_radiometric;
}


int lepton_get_model()
{
	return lep_type;
}


void lepton_agc(bool en)
{
	if (en) {
		if (lep_is_radiometric) {
			cci_set_radiometry_tlinear_enable_state(CCI_RADIOMETRY_TLINEAR_DISABLED);
		}
		cci_set_agc_enable_state(CCI_AGC_ENABLED);
	} else {
		if (lep_is_radiometric) {
			cci_set_radiometry_tlinear_enable_state(CCI_RADIOMETRY_TLINEAR_ENABLED);
		}
		cci_set_agc_enable_state(CCI_AGC_DISABLED);
	}
}


void lepton_ffc()
{
	cci_run_ffc();
}


void lepton_gain_mode(uint8_t mode)
{
	cc_gain_mode_t gain_mode;
	
	if (lep_is_radiometric) {
		switch (mode) {
			case LEP_GAIN_HIGH:
				gain_mode = LEP_SYS_GAIN_MODE_HIGH;
				break;
			case LEP_GAIN_LOW:
				gain_mode = LEP_SYS_GAIN_MODE_LOW;
				break;
			default:
				gain_mode = LEP_SYS_GAIN_MODE_AUTO;
		}
		cci_set_gain_mode(gain_mode);
	}
}


void lepton_spotmeter(uint16_t r1, uint16_t c1, uint16_t r2, uint16_t c2)
{
	if (lep_is_radiometric) {
		cci_set_radiometry_spotmeter(r1, c1, r2, c2);
	}
}


void lepton_emissivity(uint16_t e)
{
	cci_rad_flux_linear_params_t set_flux_values;
	
	if (lep_is_radiometric) {
		// Scale percentage e into Lepton scene emissivity values (1-100% -> 82-8192)
		if (e < 1) e = 1;
		if (e > 100) e = 100;
		set_flux_values.sceneEmissivity = e * 8192 / 100;
		
		// Set default (no lens) values for the remaining parameters
		set_flux_values.TBkgK      = 29515;
		set_flux_values.tauWindow  = 8192;
		set_flux_values.TWindowK   = 29515;
		set_flux_values.tauAtm     = 8192;
		set_flux_values.TAtmK      = 29515;
		set_flux_values.reflWindow = 0;
		set_flux_values.TReflK     = 29515;
		
		cci_set_radiometry_flux_linear_params(&set_flux_values);
	}
}


uint32_t lepton_get_tel_status(uint16_t* tel_buf)
{
	return (tel_buf[LEP_TEL_STATUS_HIGH] << 16) | tel_buf[LEP_TEL_STATUS_LOW];
}


/**
 * Convert a temperature reading from the lepton (in units of K * 100) to C
 */
float lepton_kelvin_to_C(uint32_t k, float lep_res)
{
	return (((float) k) * lep_res) - 273.15;
}