#include "__LTC2493_definitions.hpp"
#include "LTC2493_BellowsPosition.hpp"
#include <cstring>
#include <system_time.hpp>

#ifdef SIM_PLATFORM
#include "../../Tests/mocks/CPF_Simulation_Euler.hpp"
#endif // SIM_PLATFORM


BellowPosError LTC2493::start()
{
	// Always send config data on start
	sw_.enablePower(true);
	
	// The device uses two configuration words, which we setup here.
	uint8_t config1 = 0;
	uint8_t config2 = 0;
	
	switch (adcCfg_.chan)
	{
	case (ChannelConfig::DIFF_0P_1N): { config1 = CHANNEL_DIFF_0P_1N; break; }
	case (ChannelConfig::DIFF_2P_3N): { config1 = CHANNEL_DIFF_2P_3N; break; }
	case (ChannelConfig::DIFF_0N_1P): { config1 = CHANNEL_DIFF_0N_1P; break; }
	case (ChannelConfig::DIFF_2N_3P): { config1 = CHANNEL_DIFF_2N_3P; break; }
	case (ChannelConfig::SINGLE_0P): { config1 = CHANNEL_SINGLE_0P; break; }
	case (ChannelConfig::SINGLE_2P): { config1 = CHANNEL_SINGLE_2P; break; }
	case (ChannelConfig::SINGLE_1P): { config1 = CHANNEL_SINGLE_1P; break; }
	case (ChannelConfig::SINGLE_3P): { config1 = CHANNEL_SINGLE_3P; break; }
	default: { config1 = CHANNEL_SINGLE_0P; break; }
	}
	
	config2 |= CONFIG2_ENABLE;
	switch (adcCfg_.filt)
	{
	case (FilterConfig::EN_50HZ_REJECTION): { config2 |= CONFIG2_50HZ_REJECTION; break ;}
	case (FilterConfig::EN_60HZ_REJECTION): { config2 |= CONFIG2_60HZ_REJECTION; break; }
	case (FilterConfig::EN_50HZ_60HZ_REJECTION): { config2 |= CONFIG2_60_50HZ_REJECTION; break; }
	default: { config2 |= CONFIG2_60_50HZ_REJECTION; break;  }
	}
	
	switch (adcCfg_.speed)
	{
	case (SpeedConfig::SPEED_1X): { config2 |= CONFIG2_SPEED_1X; break; }
	case (SpeedConfig::SPEED_2X): { config2 |= CONFIG2_SPEED_2X; break; }
	default: { config2 |= CONFIG2_SPEED_1X; break; }
	}
	
	uint8_t request[] = { config1, config2 };
	uint8_t response[2]; // not really needed
	I2C::op_t op {
		.address = i2c_address_,
		.op = I2C::operation::write,
		.tx_buffer = request,
		.tx_size = 2,
		.rx_buffer = response,
		.rx_size = sizeof(response)
	};

	auto status = i2c_.transfer(op);

	if(status == I2C::status::ok)
	{
		started_ = true;
		return BellowPosError::NONE;
	}
	else
	{
		started_ = false;
		return BellowPosError::UNKNOWN_ERROR;
	}
}

BellowPosError LTC2493::stop()
{
	started_ = false;
	return BellowPosError::NONE;
}

BellowPosError LTC2493::sleep()
{
	//Note: the LTC2493 automatically goes ot sleep after it's last conversion,
	//so this is pedantic.
	sw_.enablePower(false);
	return BellowPosError::NONE;
}

BellowPosError LTC2493::requestMeasurement()
{
	BellowPosError err = BellowPosError::WAITING_FOR_CONVERSION;

	// Don't want to rely on the device's boot config. So we'll always send the config
	if (!started_)
	{
		start();
		return BellowPosError::NOT_CONFIGURED;
	}
	
#ifdef SIM_PLATFORM
	// Makes a call to the simulation to compute measurement based on simulation state
	lastMeasurement_ = get_fake_measurement();
	return BellowPosError::NONE;
#endif
	
	uint8_t txBuffer[2];	// Whouldn't be needed, but here for completeness
	uint8_t rxBuffer[4];
	union
	{
		uint8_t buf[4];
		int32_t counts;
	} data;
	
	memset(rxBuffer, 0, sizeof(rxBuffer));
	
	I2C::op_t read = {
		.address = i2c_address_,
		.op = I2C::operation::read,
		.tx_buffer = txBuffer,
		.tx_size = sizeof(txBuffer),
		.rx_buffer = rxBuffer,
		.rx_size = sizeof(rxBuffer)
	};
	
	// NOTE: The LTC2493 sleeps after completing a conversion/transmission. So if
	// the device was last read N seconds in the past, and is read again, the ADC value
	// will be correspondginly stale. You can read the device multiple times to get the
	// most up to date conversion.
	// If a conversion is in process, but not ready, the device will NACK, which corresponds
	// to I2C::status::addrNACK.
	auto status = i2c_.transfer(read);
	
	if (status == I2C::status::ok)
	{
		//Got a good measurment, do the converstion
		//Check for out of range measurements. See datasheet for details.
		switch (rxBuffer[0])
		{
		case (0xC0): { err = BellowPosError::OUT_OF_RANGE; return err; }	// VIN >= FS
		case (0x3F): { err = BellowPosError::OUT_OF_RANGE; return err; }	// VIN < –FS
		default: { break; }
		}
		
		//Data is transmitted big-endian, so swap to little-endian
		data.buf[0] = rxBuffer[3];
		data.buf[1] = rxBuffer[2];
		data.buf[2] = rxBuffer[1];
		data.buf[3] = rxBuffer[0];
		
		// Get rid of the sub-LSBs
		data.counts = (data.counts & 0x7FFFFFFF) >> 6;

		if (0x01000000 & data.counts)
		{
			data.counts |= int32_t(0xFF000000);
		}
		
		// Update the measurement struct for retrival
		lastMeasurement_.tickstamp = systime::getDateTimeNow();
		lastMeasurement_.counts = data.counts;
		lastMeasurement_.volts = counts2volts(lastMeasurement_.counts, adcCfg_.vRefNeg_volts, adcCfg_.vRefPos_volts);
		lastMeasurement_.position_mm = volts2bellowsPosition_mm(lastMeasurement_.volts, sensorCfg_.sensor_slope_mm_per_volt, sensorCfg_.sensor_offset_mm);
		lastMeasurement_.position_pctFull = bellowsPosition_mm2percent(lastMeasurement_.position_mm, sensorCfg_.positionLowLim_mm, sensorCfg_.positionHighLim_mm);
		lastMeasurement_.conversion_good = true;
		err = BellowPosError::NONE;
	}
	else if (status == I2C::status::addrNACK)
	{
		// If the device doesn't ACK, that indicates it's currently performing a conversion, and is not ready to be read.
		// Don't need to mark the previous conversoin good, since that's not necessarily true
		err = BellowPosError::WAITING_FOR_CONVERSION;
	}
	else
	{	
		// If not waiting, nor have received a good transaction, assume soemthing is wrong.
		lastMeasurement_.conversion_good = false;
		err = BellowPosError::UNKNOWN_ERROR;
	}

	logEngLine(lastMeasurement_, "requestMeasurement() status: %d" + std::to_string(static_cast<uint8_t>(status)));
	
	return err;
}


float LTC2493::counts2volts(const int32_t counts, const float refNegVolts, const float refPosVolts)
{
	float vFS = (refPosVolts - refNegVolts) / 2;	// Vref / 2
	
	float adcVolts = float(counts) * vFS / 16777216.0f; // 2^24

	return adcVolts;
}

float LTC2493::volts2bellowsPosition_mm(const float sensorVolts, const float slope, const float intercept)
{
	float mm = sensorVolts * slope + intercept;
	
	return mm;
}

float LTC2493::bellowsPosition_mm2percent(const float pos, const float lowLim_mm, const float highLim_mm)
{
	float percent = (pos - lowLim_mm) / (highLim_mm - lowLim_mm);
	
	return percent;
}

bool LTC2493::isWithinLimits(bool takeNewMeasurement)
{
	BellowPosError ret = BellowPosError::WAITING_FOR_CONVERSION;
	bool within_limits = false;
	
	// Check if we need to make a new measurement
	if (takeNewMeasurement || !measurement_taken_since_start_)
	{
		ret = requestMeasurement();
	}
	
	if (lastMeasurement_.conversion_good && ret == BellowPosError::NONE)
	{
		if ((lastMeasurement_.position_mm > sensorCfg_.positionLowLim_mm) 
			&& (lastMeasurement_.position_mm < sensorCfg_.positionHighLim_mm))
		{
			within_limits = true;
		}
	}
	else
	{
		// Last measurement was bad...
		within_limits = false;
	}
	
	return within_limits;
}

#ifdef SIM_PLATFORM
BellowsPosition::Measurement LTC2493::get_fake_measurement()
{
	BellowsPosition::Measurement m = { };
	m.conversion_good = true;
	m.position_mm = CPF_SIM_Euler::get_instance()->get_simulated_inner_bellows_position_mm();
	m.position_pctFull = bellowsPosition_mm2percent(m.position_mm, sensorCfg_.positionLowLim_mm, sensorCfg_.positionHighLim_mm);
	m.tickstamp = systime::getDateTimeNow();
	return m;
}
#endif