#include <iostream>
using namespace std;
	
#include "BuoyancyEngine.hpp"
#include "SDLogger.hpp"
#include "SBE41.hpp"

SDLogger sdLogger = SDLogger::getInstance();
//TODO I'm not sure passing an arugment to ::getInstance makes sense after the initial instantiation
SBE41 ctd1 = SBE41::getInstance("CTD1");

//#include "mcuRealTimeClock.hpp"
mcuRealTimeClock mcuRTC_BcyEngine = mcuRealTimeClock::getInstance();

BuoyancyEngine::BuoyancyEngine()
{
	cout << "\t\tBuoyancyEngine constructor" << endl;
	InitSBE41Timer();
}

void BuoyancyEngine::InitSBE41Timer()
{
	__TIM2_CLK_ENABLE();
	hBE_CTDTimer.Init.Prescaler = 40000;
	hBE_CTDTimer.Init.CounterMode = TIM_COUNTERMODE_UP;
	hBE_CTDTimer.Init.Period = 10000;
	hBE_CTDTimer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
	hBE_CTDTimer.Init.RepetitionCounter = 0;
	HAL_TIM_Base_Init(&hBE_CTDTimer);

	HAL_NVIC_SetPriority(TIM2_IRQn, 0, 0);
	HAL_NVIC_EnableIRQ(TIM2_IRQn);
	
	cout << "\t\tInitialized SBE41 Timer" << endl;
}

void BuoyancyEngine::StartSBE41Timer()
{
	HAL_TIM_Base_Start_IT(&hBE_CTDTimer);
}

void BuoyancyEngine::StopSBE41Timer()
{
	HAL_TIM_Base_Stop_IT(&hBE_CTDTimer);
}
	
	
	
extern "C" void TIM2_IRQHandler()
{
	//cout << "Hit TIM2_IRQHandler" << endl;
	HAL_TIM_IRQHandler(&hBE_CTDTimer);
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	char engrMsg[256] = "";
	string retTimestamp;
	string retMsgString;

	snprintf(engrMsg, sizeof(engrMsg), "SBE41 Sending fast pressure at time: %s", mcuRTC_BcyEngine.getCPFTimestamp().c_str());
	sdLogger.logEngrMsg(engrMsg, sizeof(engrMsg));

	ctd1.sendCommand(SBE41::getFastPress, retTimestamp, &retMsgString);
}

float BuoyancyEngine::decreaseBuoyancy()
{
	cout << "\t\tBuoyancyEngine decrease buoyancy" << endl;
	return (88.123);
}
float BuoyancyEngine::getBellowsPosition()
{
	cout << "\t\tBuoyancyEngine getBellowsPosition" << endl;
	return (99.234);
}
float BuoyancyEngine::increaseBuoyancy()
{
	cout << "\t\tBuoyancyEngine increase buoyancy" << endl;
	return (123.456);
}

float BuoyancyEngine::runPID(float pressure)
{
	cout << "\t\tBuoyancyEngine runPID" << endl;
	return (456.789);
}

float BuoyancyEngine::stopMotor()
{
	cout << "\t\tBuoyancyEngine stopMotor" << endl;
	return (0.0);
}