/**
 * prox_tmd2635.c
 *
 * Low level control of the ams TMD2635 proximity sensor over I2C.
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */

#include <asf.h>
#include "i2c.h"
#include "prox_tmd2635.h"
#include <stddef.h>
#include <stdbool.h>

bool prox_connected = false;
Prox_Settings proxsettings;

uint8_t tmd2635_init(I2C_Handle* hi2c, bool softreset) {
	I2C_Transaction_Request i2ctxn;
	uint8_t dummy_data;
	uint8_t retval = pdPASS;
	
	dummy_data = 0;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.internal_addr = 0;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.len = 1;
	i2ctxn.data = &dummy_data;
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2ctxn.cb = NULL;
	// Dummy write to initialize chip
	i2c_lock(hi2c);
	i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	// No need to check for reply. if this is the first access after
	// power up, the TMD2635 won't ACK here. This just allows it
	// to set its address properly
	
	if(softreset) {
	// Execute soft reset, write bit SOFTRST (0x01) in register SOFTRST (0xA8)
		i2ctxn.internal_addr = PROX_REG_SOFTRST;
		dummy_data = PROX_SOFTRST_RST;
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}
	
	// Turn on power to the chip
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2ctxn.internal_addr = PROX_REG_ENABLE;
	dummy_data = PROX_ENABLE_PON;
	if(retval == pdPASS) {
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}
	
	// Get the current PCFG1 reg
	i2ctxn.internal_addr = PROX_REG_PCFG1;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	if(retval == pdPASS) {
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}
	
	// Set PLDRIVE to 7mA
	dummy_data &= ~(PROX_PCFG1_PLDRIVE_Msk);
	dummy_data |= PROX_PCFG1_PLDRIVE_7mA;
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	if(retval == pdPASS) {
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}
	
	// Set photodiode to "FAR" only. This rejects more of the reflection from the IR sensor lens
	i2ctxn.internal_addr = PROX_REG_CFG8;
	dummy_data = PROX_CFG8_PDSELECT_FAR;
	if(retval == pdPASS) {
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}
	
	// Datasheet says Test9 is required to be set.
	dummy_data = PROX_TEST9_VAL;
	i2ctxn.internal_addr = PROX_REG_TEST9;
	if(retval == pdPASS) {
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}

	// Enable proximity detection
	// Set PON (0x01) and PEN (0x04) in ENABLE (0x80)
	dummy_data = PROX_ENABLE_PEN | PROX_ENABLE_PON; 
	i2ctxn.internal_addr = PROX_REG_ENABLE;
	if(retval == pdPASS) {
		retval = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}

	i2c_unlock(hi2c);
	if(retval == pdPASS) {
		prox_connected = true;
	} else {
		prox_connected = false;
	}

	return retval;
}


uint16_t tmd2635_get_distance(I2C_Handle* hi2c) {
	I2C_Transaction_Request i2ctxn;
	uint8_t dist_buf[2];
	uint8_t errchk;
	
	i2ctxn.cb = NULL;
	i2ctxn.data = dist_buf;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.internal_addr = PROX_REG_PDATAL; 
	i2ctxn.len = 2; // Reads both PDATAL and PDATAH
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	
	i2c_lock(hi2c);
	errchk = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	i2c_unlock(hi2c);
	if(pdPASS == errchk) {
		return ((uint16_t)dist_buf[0]) + (((uint16_t)dist_buf[1]) << 8);
	} else {
		prox_connected = false;
		return 0;
	}
}

uint16_t tmd2635_get_offset(I2C_Handle* hi2c) {
	I2C_Transaction_Request i2ctxn;
	uint8_t offset_buf[2];
	uint8_t errchk;
	
	i2ctxn.cb = NULL;
	i2ctxn.data = offset_buf;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.internal_addr = PROX_REG_POFFSETL; // POFFSETL
	i2ctxn.len = 2; // reads both POFFSETL and POFFSETH
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_lock(hi2c);
	errchk = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	i2c_unlock(hi2c);
	if(pdPASS == errchk) {
		return ((uint16_t)offset_buf[0]) + (((uint16_t)offset_buf[1]) << 8);
	} else {
		prox_connected = false;
		return 0;
	}
}

uint8_t tmd2635_set_offset(I2C_Handle* hi2c, uint16_t new_offset) {
	I2C_Transaction_Request i2ctxn;
	uint8_t offset_buf[2];
	uint8_t errchk;
	i2ctxn.cb = NULL;
	i2ctxn.data = offset_buf;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.internal_addr = PROX_REG_POFFSETL; // POFFSETL
	i2ctxn.len = 2; // writes both POFFSETL and POFFSETH
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;

	offset_buf[0] = (uint8_t)(new_offset & 0x00FFu);
	offset_buf[1] = (uint8_t)((new_offset & 0xFF00u) >> 8);

	i2c_lock(hi2c);
	errchk = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	i2c_unlock(hi2c);
	if(pdFAIL == errchk) {
		prox_connected = false;
	}
	return errchk;
}

bool tmd2635_calibrate(I2C_Handle* hi2c, Cal_Target_T tgt)
{
	I2C_Transaction_Request i2ctxn;
	uint8_t databuf;
	uint8_t errchk;
	bool calib_complete = false;
	
	i2ctxn.cb = NULL;
	i2ctxn.data = &databuf;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.internal_addr = PROX_REG_CALIBCFG; // CALIBCFG
	i2ctxn.len = 1;
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	databuf = PROX_CALIBCFG_TARGET(tgt) | PROX_CALIBCFG_RSVD; // Calib Target = function input, AUTO_OFFSET_ADJ is off, no ADC averaging
	i2c_lock(hi2c);
	errchk = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	
	i2ctxn.internal_addr = PROX_REG_CALIB;
	databuf = PROX_CALIB_START | PROX_CALIB_ELECTRICAL; // Calib start, only electrical xtalk, no hardware averaging, PRATE ignored
	if(errchk == pdPASS) {
		errchk = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	}
	i2c_unlock(hi2c);
	// Now wait until calibration is complete
	i2ctxn.internal_addr = PROX_REG_CALIBSTAT; // Calib stat
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	uint16_t calib_wait = 0;
	while((errchk == pdPASS) && (calib_complete == false) && (calib_wait < 10)) {
		// little wait time since calibration can take up to 150ms after cold start
		vTaskDelay(25);

		i2c_lock(hi2c);
		errchk = i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
		i2c_unlock(hi2c);
		
		if((databuf & PROX_CALIBSTAT_CALIB_FINISHED) != 0) {
			calib_complete = true;	
		}

		calib_wait++;
	}
	
	return calib_complete;
}

bool tmd2635_is_connected() {
	return prox_connected;
}


Prox_Settings* tmd2635_get_settings(void) {
	return &proxsettings;
}

bool tmd2635_apply_settings(I2C_Handle* hi2c) {
	// Applies the settings in proxsettings struct
	// Assumes that the calling function has already modified the struct
	I2C_Transaction_Request i2ctxn;
	uint8_t temp_reg;
	
	// Check values before applying...
	// PWEN is just a bool, anything besides zero is true
	// PRATE is any allowable byte
	// PWLONG is a bool, anything besides zero is true
	// PGAIN can be 0 to 3
	if(proxsettings.pgain > 3) {
		return false;
	}
	// PPULSE is any number 0 to 63
	if(proxsettings.ppulse > 63) return false;
	// PLEN is 0 to 7
	if(proxsettings.plen > 7) return false;
	// PLDRIVE can be 2mA to 10mA. The value sent to the register is 0 to 8, 0=2mA and 8=10mA.
	if(proxsettings.pldrive > 8) return false;
	// PWTIME is any byte
	// PDSELECT can be 1, 2, or 3 (far, near, or both)
	if((proxsettings.pdselect != Prox_PD_Far) && (proxsettings.pdselect != Prox_PD_Near) && (proxsettings.pdselect != Prox_PD_Both)) {
		return false;
	}
	// PMAVG can be 0 through 3 (1, 2, 4, or 8 averages)
	if(proxsettings.pmavg > 3) return false;
	// Prox average can be 0 to 7 (1, 2, 4, 8, 16, 32, 64, or 128 samples averaged)
	if(proxsettings.proxavg > 7) return false;
	
	
	i2ctxn.cb = NULL;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.data = &temp_reg;
	i2ctxn.len = 1;	
	i2c_lock(hi2c);
	
	// PWEN
	i2ctxn.internal_addr = PROX_REG_ENABLE;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	if(proxsettings.pwen) {
		temp_reg |= PROX_ENABLE_PWEN;
	} else {
		temp_reg &= ~(PROX_ENABLE_PWEN);
	}
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PRATE
	i2ctxn.internal_addr = PROX_REG_PRATE;
	temp_reg = proxsettings.prate;
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PWLONG
	i2ctxn.internal_addr = PROX_REG_CFG0;
	temp_reg = PROX_CFG0_RSVD;
	if(proxsettings.pwlong) {
		temp_reg |= PROX_CFG0_PWLONG;
	}
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PGAIN and PPULSE
	i2ctxn.internal_addr = PROX_REG_PCFG0;
	temp_reg = PROX_PCFG0_PGAIN(proxsettings.pgain) | PROX_PCFG0_PPULSE(proxsettings.ppulse);
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PPULSELEN and PLDRIVE
	i2ctxn.internal_addr = PROX_REG_PCFG1;
	temp_reg = PROX_PCFG1_PPULSELEN(proxsettings.plen) | PROX_PCFG1_PLDRIVE(proxsettings.pldrive);
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PWTIME
	i2ctxn.internal_addr = PROX_REG_PWTIME;
	temp_reg = proxsettings.pwtime;
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PDSELECT
	i2ctxn.internal_addr = PROX_REG_CFG8;
	switch(proxsettings.pdselect) {
		case Prox_PD_Far:
			temp_reg = PROX_CFG8_PDSELECT_FAR;
		break;
		case Prox_PD_Near:
			temp_reg = PROX_CFG8_PDSELECT_NEAR;
		break;
		case Prox_PD_Both:
			temp_reg = PROX_CFG8_PDSELECT_BOTH;
		break;	
		default:
			return false;
		break;
	}
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PMAVG
	i2ctxn.internal_addr = PROX_REG_PFILTER;
	temp_reg = PROX_PFILTER_PMAVG(proxsettings.pmavg);
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	// PROXAVG
	i2ctxn.internal_addr = PROX_REG_CALIBCFG;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	temp_reg &= ~(PROX_CALIBCFG_AVG_Msk);
	// Ensure the reserved bit is set
	temp_reg |= PROX_CALIBCFG_RSVD;
	temp_reg |= PROX_CALIBCFG_AVG(proxsettings.proxavg);
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	if(pdPASS != i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME)) {
		i2c_unlock(hi2c);
		return false;
	}
	
	i2c_unlock(hi2c);
	return true;
}