#define SIMPRESSURE

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

static bool ctdSBE41MsgReady = false;

const uint8_t ctdSBE41QWidth = 64;
const uint8_t ctdSBE41QDepth = 32;
struct ctdSBE41FIFOStruct
{
	uint64_t timeStamp;
	uint8_t msg[ctdSBE41QWidth];
};

static uint8_t ctdSBE41RXBuffer[ctdSBE41QWidth];

// TODO need a better way to add the 5 parameter
alignas(32) static const uint8_t startProfile[] = "startProfile5\r";
alignas(32) static const uint8_t fastPressure[] = "fp\r\n";
// TODO: is the one below really necessary?
alignas(32) static const uint8_t fastPressure_extended[] = "S> FP 1.234\r\n";
alignas(32) static const uint8_t sim_fast_pressure_string[] =
	"SBE 41CP V 7.2.5\r\nS > fp\r\n   - 0.20";

using std::cout;
using std::endl;

FIFOQueue<ctdSBE41FIFOStruct, ctdSBE41QDepth> ctdSBE41FIFO = {};

int8_t ctdSBE41::getPressure(CTDSample& currentSample)
{
	if(ctdSBE41RXTXInProgress)
	{
		if(ctdSBE41FIFO.getCount() > 0)
		{
			ctdSBE41FIFOStruct fifoQRecord;
			ctdSBE41FIFO.dequeue(fifoQRecord);
			if(std::equal(fifoQRecord.msg, fifoQRecord.msg + 2, "fp"))
			{
#ifdef SIMPRESSURE
				simFastPressure(currentSample);
#elif
				parseFastPressure(currentSample);
#endif
				ctdLogEngLine(currentSample, "getPressure:");
			}
			else
			{
				// TODO log unexpected message to unexpected msg queue
			}
			if(ctdSBE41FIFO.getCount() > 0)
			{
				// TODO deal with jabbering GPS
			}
			ctdSBE41RXTXInProgress = false;

			return 1;
		}
		else
		{
			return 0;
		}
	}
	else
	{
		// TODO need to log anything in the buffer and probably terminate the hanging
		// HAL_UART_Receive_IT
		clearSample(currentSample);

		// cout << "TX/RX CTD fast pressure" << endl;

		uart_.receive(
			ctdSBE41RXBuffer, ctdSBE41QWidth,
			[](UART::status result) noexcept {
				// TODO: implement!
				(void)result;
			},
			std::chrono::milliseconds(250)); // TODO: set the message timeout based on your goals
		uart_.transmit(fastPressure, sizeof(fastPressure) - 1); // subtract 1 to avoid null char
		ctdSBE41RXTXInProgress = true;

		return 0;
	}
}

void ctdSBE41::sendFP()
{
	if(ctdSBE41RXTXInProgress == false)
	{
		ctdSBE41MsgReady = false;

		// cout << "TX/RX CTD fast pressure" << endl;
		uart_.receive(
			ctdSBE41RXBuffer, ctdSBE41QWidth,
			[this](UART::status result) noexcept {
				// TODO: implement!
				(void)result;

				// if error, need to set ctdSBE41RXTXInProgress = false
				ctdSBE41RXTXInProgress = false;
			},
			std::chrono::milliseconds(250)); // TODO: set the message timeout based on your goals
		uart_.transmit(fastPressure_extended,
					   sizeof(fastPressure_extended) - 1); // subtract 1 to avoid null char

		ctdSBE41MsgReady = false;
		ctdSBE41RXTXInProgress = true;
	}
}

int8_t ctdSBE41::simGetPressure(CTDSample& ctdSample)
{
	static float currentPressure = 0.0;
	static const double pressureRate = 0.1;
	static const float stopPressure = 100.0;
	static uint64_t lastTickCount = systime::getUptimeMilliseconds();
	uint64_t currentTickCount = systime::getUptimeMilliseconds();
	char simMsg[32] = "";
	static bool firstSim = true;

	if(ctdSBE41RXTXInProgress)
	{
		// cout << "Processing CTD msg" << endl;
		currentTickCount = systime::getUptimeMilliseconds();
		if(firstSim)
		{
			lastTickCount = currentTickCount;
			firstSim = false;
		}

		if(currentPressure < stopPressure)
		{
			currentPressure =
				(currentPressure) +
				static_cast<float>(pressureRate *
								   static_cast<double>((static_cast<float>(currentTickCount) -
														static_cast<float>(lastTickCount)) /
													   2000.0f)); // TODO why 2000 and not 1000?
		}

		lastTickCount = currentTickCount;

		memset(simMsg, 0, sizeof(simMsg));
		snprintf(simMsg, sizeof(simMsg), "S> \r SBE41cp \r %6.2f",
				 static_cast<double>(currentPressure));

		clearSample(ctdSample);
		for(int i = 0; static_cast<uint16_t>(i) < sizeof(simMsg); i++)
		{
			ctdSample.msg[i] = static_cast<uint8_t>(simMsg[i]);
		}

		ctdSBE41RXTXInProgress = false;
		return 1;
	}
	else
	{
		// TODO need to log anything in the buffer and probably terminate the hanging
		// HAL_UART_Receive_IT
		clearSample(ctdSample);
		ctdSBE41MsgReady = false;

		// TODO this is just until I figure out how to do _IT and/or _DMA with multiple UARTS
		// cout << "TX/RX CTD fast pressure" << endl;

		uart_.receive(
			ctdSBE41RXBuffer, ctdSBE41QWidth,
			[](UART::status result) noexcept {
				// TODO: implement!
				(void)result;
			},
			std::chrono::seconds(1)); // TODO: set the message timeout based on your goals
		uart_.transmit(fastPressure, sizeof(fastPressure) - 1); // subtract 1 to avoid null char

		ctdSBE41RXTXInProgress = true;

		return 0;
	}
}

int8_t ctdSBE41::setCPMode()
{
	if(ctdSBE41RXTXInProgress)
	{
		if(ctdSBE41MsgReady)
		{
			ctdSBE41RXTXInProgress = false;

			return 1;
		}
		else
		{
			return 0;
		}
	}
	else
	{
		// TODO need to log anything in the buffer and probably terminate the hanging
		//		memset(ctdSBE41RXBuffer, 0, sizeof(ctdSBE41RXBuffer));
		//		memset(ctdSBE41MsgBuffer, 0, sizeof(ctdSBE41MsgBuffer));
		ctdSBE41MsgReady = false;
		//		ctdSBE41MsgBufferIndex = 0;

		uart_.receive(
			ctdSBE41RXBuffer, ctdSBE41QWidth,
			[](UART::status result) noexcept {
				// TODO: implement!
				(void)result;
			},
			std::chrono::seconds(1)); // TODO: set the message timeout based on your goals
		uart_.transmit(startProfile, sizeof(startProfile) - 1); // subtract 1 to avoid null char

		ctdSBE41RXTXInProgress = true;

		return 0;
	}
}

void ctdSBE41::defineCommands()
{
}

int8_t ctdSBE41::parseFastPressure(CTDSample& currentsample)
{
	(void)currentsample;
	return -2;
}

int8_t ctdSBE41::simFastPressure(CTDSample& currentSample)
{
	memcpy(currentSample.msg, sim_fast_pressure_string, sizeof(sim_fast_pressure_string));
	currentSample.pressure_dBar = -0.20f;
	currentSample.salinityPPS = 35.0;
	currentSample.tempDegC = 20.0;
	currentSample.sysTickCount = systime::getUptimeMilliseconds();

	return 1;
}

void ctdSBE41::handlectdSBE41FPTimer()
{
	sendFP();
	// cout << "ctdSBE41:handlectdSBE41FPTimer()" << endl;
	// cout << "CTD1:handleCTD1FPTimer()" << endl;
}

void ctdSBE41::start()
{
	ctdSBE41Timer.period(std::chrono::seconds(3));
	ctdSBE41Timer.config(Timer::Config::periodic);
	ctdSBE41Timer.registerCallback(std::bind(&ctdSBE41::handlectdSBE41FPTimer, this));
}

void ctdSBE41::stop()
{
	ctdSBE41Timer.registerCallback(nullptr);
}

bool ctdSBE41::doSelftest()
{
	return true;
}
