#include "BellowsPositionADC2485.hpp"
#include <system_time.hpp>

BellowsPositionADC2485::BellowsPositionADC2485(I2C& i2c, std::string id, DevicePowerSwitch& sw,
											   const float lowLim_mm, const float highLim_mm,
											   const uint8_t address) :
	BellowsPosition(id, lowLim_mm, highLim_mm),
	i2c_(i2c), sw_(sw), currentState_(LTC2485::NOT_READY), lastBellowsPosition_mm(0.0),
	initComplete_(false), address_(address), lastAdcCounts_(0), timeLastConversion_ms_(0)
{
}

BError BellowsPositionADC2485::start()
{
	BError err = BError::NOT_CONFIGURED;
	if(initComplete_)
	{
		sw_.enablePower(true);
		currentState_ = LTC2485::READY;
		err = BError::NONE;
	}
	return err;
}

BError BellowsPositionADC2485::init(bellowsADCConfig_t cfg)
{
	cfg_ = cfg;
	initComplete_ = true;

	return BError::NONE;
}

BError BellowsPositionADC2485::stop()
{
	return BError::NONE;
}

BError BellowsPositionADC2485::sleep()
{
	// According to documentation, the ADC automatically enters
	// a low-power sleep state after returning a conversion. This is
	// a placeholder.
	sw_.enablePower(false);
	return BError::NONE;
}

BError BellowsPositionADC2485::getPosition_pctFull(float& percentOfFull)
{
	float position_mm = 0.0f;
	BError ret = getPosition_mm(position_mm);

	percentOfFull = 100.0f * (1.0f - (highLim_mm_ - position_mm) / (highLim_mm_ - lowLim_mm_));
	return ret;
}

BError BellowsPositionADC2485::getPosition_mm(float& currentPosition_mm)
{
	if(!initComplete_)
	{
		return BError::NOT_CONFIGURED;
	}

	BError err = BError::UNKNOWN_ERROR;

	switch(currentState_)
	{
		case(LTC2485::READY):
		{
			// setup and start the I2C transaction. State transition is
			// handled by initiateConversion()
			initiateConversion();
			err = BError::WAITING_FOR_DATA;
			break;
		}
		case(LTC2485::WAITING_ON_CB):
		{
			// Do nothing. Callback lambda will adjust state when appropriate
			err = BError::WAITING_FOR_DATA;
			break;
		}
		case(LTC2485::PROCESS_NEW_BYTES):
		{
			// Check for out of range measurements. See datasheet for details.
			if(rxBuffer_[0] >= 0xC0)
			{
				// Vin >= Fs
				logLine("[ERROR] 2485 Vin>=Fs");
				lastAdcCounts_ = INT32_MAX;
				err = BError::OUT_OF_RANGE;
			}

			if(rxBuffer_[0] < 0x40)
			{
				// Vin < -Fs
				logLine("[ERROR] 2485 Vin<-Fs");
				lastAdcCounts_ = INT32_MIN;
				err = BError::OUT_OF_RANGE;
			}

			adcCounts2Bellows_mm(lastBellowsPosition_mm, lastAdcCounts_);
			currentState_ = LTC2485::WAITING_FOR_NEXT_CALL_PERIOD;
			err = BError::NONE;
			break;
		}
		case(LTC2485::WAITING_FOR_NEXT_CALL_PERIOD):
		{
			currentState_ = gotoReadyIfMillisecondsPassed(RETRY_PERIOD_MS);
			break;
		}
		case(LTC2485::NOT_READY):
		{
			// nothing to do...
			break;
		}
		default:
		{
			// error
			break;
		}
	}

	currentPosition_mm = lastBellowsPosition_mm;

	return err;
}

const std::string BellowsPositionADC2485::getEngFileHeader()
{
	return std::string(moduleID_.data()) + "\t" + engFileHeader1_.data() + "\r\n$\t" +
		   (moduleID_.data()) + "\t" + engFileHeader2_.data();
}

void BellowsPositionADC2485::initiateConversion()
{
	// Transmit desired config
	const uint8_t tx_buffer = LTC2485::EXT_INPUT_60HZ_AUTOCAL;
	I2C::op_t op = {.address = address_,
					.op = I2C::operation::writeRead,
					.tx_buffer = &tx_buffer,
					.tx_size = sizeof(tx_buffer),
					.rx_buffer = rxBuffer_,
					.rx_size = sizeof(rxBuffer_)};

	i2c_.transfer(op, [this](I2C::status result) {
		if(result == I2C::status::ok)
		{
			this->lastAdcCounts_ =
				this->int32FromBytesDCBA(this->rxBuffer_, sizeof(this->rxBuffer_));
			this->timeLastConversion_ms_ = systime::getUptimeMilliseconds();
			this->currentState_ = LTC2485::States::PROCESS_NEW_BYTES;
		}
		else
		{
			// Transaction timeout
			this->currentState_ = LTC2485::States::WAITING_FOR_NEXT_CALL_PERIOD;
		}
	});
}

void BellowsPositionADC2485::adcCounts2Bellows_mm(float& position_mm, int32_t adcCounts)
{
	// See LTC2485 datasheet for details on the below conversion

	// Invert MSB, result is 2's compliment
	adcCounts = static_cast<int32_t>(static_cast<uint32_t>(adcCounts) ^ 0x80000000);

	float adcVolts = float(adcCounts) * cfg_.vRefADC_volts / 2147483648.0f; // 2^31

	position_mm = (cfg_.slope_mm_per_volt * adcVolts) + cfg_.offset_mm;
}

LTC2485::States BellowsPositionADC2485::gotoReadyIfMillisecondsPassed(const uint32_t period_ms)
{
	uint64_t now = systime::getUptimeMilliseconds();

	if(now - timeLastConversion_ms_ > period_ms)
	{
		return LTC2485::States::READY;
	}
	else
	{
		return LTC2485::States::WAITING_FOR_NEXT_CALL_PERIOD;
	}
}
