#include "LTC2946_EnergyMonitor.hpp"
#include "__LTC2946_definitions.hpp"
#include <system_time.hpp>
#include "__LTC2946_LinearDriver.hpp"

/// This macro controls what the request voltage returns.
/// Do you want to measure VIN (VDD), or ADIN?
#define REQUEST_VOLTAGE_USES_VIN 0

constexpr unsigned BUSY_RETRY_LIMIT = 50;

// LSB Weights - these might need adjusted against a reference source
// They were initially imported from the DC2156.ino reference file
// Provided by LT within the "Linduino" framework
constexpr float LTC2946_ADIN_lsb = 5.001221E-04f; //!< Typical ADIN lsb weight in volts
constexpr float LTC2946_DELTA_SENSE_lsb = 2.5006105E-05f; //!< Typical Delta lsb weight in volts
constexpr float LTC2946_VIN_lsb = 2.5006105E-02f; //!< Typical VIN lsb weight in volts
constexpr float LTC2946_Power_lsb =
	6.25305E-07f; //!< Typical POWER lsb weight in V^2 VIN_lsb * DELTA_SENSE_lsb
constexpr float LTC2946_ADIN_DELTA_SENSE_lsb =
	1.25061E-08f; //!< Typical sense lsb weight in V^2  *ADIN_lsb * DELTA_SENSE_lsb
constexpr float LTC2946_INTERNAL_TIME_lsb =
	4101.00f / 250000.00f; //!< Internal TimeBase lsb. Use LTC2946_TIME_lsb if an external CLK is
						   //!< used. See Settings menu for how to calculate Time LSB.
constexpr float LTC2946_TIME_lsb =
	16.39543E-3f; //!< Static variable which is based off of the default clk frequency of 250KHz.

void LTC2946::start()
{
	// TODO: any configuration you want here
}

void LTC2946::stop()
{
	// TODO?
}

#define RETURN_IF_FAILED(fail_var) \
	if(fail_var)                   \
		return false;

// TODO: timeout
bool LTC2946::requestCurrent()
{
	unsigned retry_count = 0;
	auto failed = LTC2946_write(i2c_, i2c_address_, LTC2946_CTRLA_REG,
								(LTC2946_CHANNEL_CONFIG_SNAPSHOT | LTC2946_DELTA_SENSE));
	RETURN_IF_FAILED(failed);

	// Loop until not busy
	uint8_t busy;
	do
	{
		failed = LTC2946_read(i2c_, i2c_address_, LTC2946_STATUS2_REG, &busy);
		RETURN_IF_FAILED(failed);
	} while((LTC2946_STATUS2_BUSY_FLAG & busy) && (retry_count++ < BUSY_RETRY_LIMIT));

	if(retry_count >= BUSY_RETRY_LIMIT)
	{
		return false;
	}

	uint16_t current_code;
	failed = LTC2946_read_12_bits(i2c_, i2c_address_, LTC2946_DELTA_SENSE_MSB_REG, &current_code);
	RETURN_IF_FAILED(failed);

	latest_current_.reading =
		LTC2946_code_to_current(current_code, resistor_val_, LTC2946_DELTA_SENSE_lsb);
	latest_current_.captureTimestamp = systime::getUptimeMilliseconds();

	return true;
}

// TODO: timeout
bool LTC2946::requestVoltage()
{
#if REQUEST_VOLTAGE_USES_VIN
	unsigned retry_count = 0;
	auto failed = LTC2946_write(i2c_, i2c_address_, LTC2946_CTRLA_REG,
								(LTC2946_CHANNEL_CONFIG_SNAPSHOT | LTC2946_VDD));
	RETURN_IF_FAILED(failed);

	// Loop until not busy
	uint8_t busy;
	do
	{
		failed = LTC2946_read(i2c_, i2c_address_, LTC2946_STATUS2_REG, &busy);
		RETURN_IF_FAILED(failed);
	} while((LTC2946_STATUS2_BUSY_FLAG & busy) && (retry_count++ < BUSY_RETRY_LIMIT));

	if(retry_count >= BUSY_RETRY_LIMIT)
	{
		return false;
	}

	uint16_t VDD_code;
	failed = LTC2946_read_12_bits(i2c_, i2c_address_, LTC2946_VIN_MSB_REG, &VDD_code);
	RETURN_IF_FAILED(failed);

	latest_voltage_.reading = LTC2946_VIN_code_to_voltage(VDD_code, LTC2946_VIN_lsb);
	latest_voltage_.captureTimestamp = systime::getUptimeMilliseconds();

	return true;
#else
	unsigned retry_count = 0;
	auto failed = LTC2946_write(i2c_, i2c_address_, LTC2946_CTRLA_REG,
								(LTC2946_CHANNEL_CONFIG_SNAPSHOT | LTC2946_ADIN));
	RETURN_IF_FAILED(failed);

	// Loop until not busy
	uint8_t busy;
	do
	{
		failed = LTC2946_read(i2c_, i2c_address_, LTC2946_STATUS2_REG, &busy);
		RETURN_IF_FAILED(failed);
	} while((LTC2946_STATUS2_BUSY_FLAG & busy) && (retry_count++ < BUSY_RETRY_LIMIT));

	if(retry_count >= BUSY_RETRY_LIMIT)
	{
		return false;
	}

	uint16_t VDD_code;
	failed = LTC2946_read_12_bits(i2c_, i2c_address_, LTC2946_ADIN_MSB_REG, &VDD_code);
	RETURN_IF_FAILED(failed);

	latest_voltage_.reading = LTC2946_ADIN_code_to_voltage(VDD_code, LTC2946_ADIN_lsb) * adin_scale_;
	latest_voltage_.captureTimestamp = systime::getUptimeMilliseconds();

	return true;
#endif
}
