/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"	// Precompiled headers
//
#include <mrpt/comms/net_utils.h>
#include <mrpt/core/bits_math.h>
#include <mrpt/hwdrivers/CServoeNeck.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/CMessage.h>

#include <cmath>
#include <cstdint>
#include <thread>

using namespace mrpt;
using namespace mrpt::comms;
using namespace mrpt::hwdrivers;
using namespace mrpt::serialization;
using namespace std::literals;

CServoeNeck::CServoeNeck() : m_usbSerialNumber("eNeck001"), m_PrevAngles(0)
{
	m_offsets.resize(3, 0);
}

CServoeNeck::~CServoeNeck() = default;

bool CServoeNeck::queryFirmwareVersion(std::string& out_firmwareVersion)
{
	try
	{
		mrpt::serialization::CMessage msg, msgRx;

		// Try to connect to the device:
		if (!checkConnectionAndConnect()) return false;

		msg.type = 0x10;
		archiveFrom(*this).sendMessage(msg);

		if (archiveFrom(*this).receiveMessage(msgRx))
		{
			msgRx.getContentAsString(out_firmwareVersion);
			std::this_thread::sleep_for(200ms);
			return true;
		}
		else
			return false;
	}
	catch (...)
	{
		Close();
		return false;
	}
}  // end-queryFirmwareVersion

/*-------------------------------------------------------------
					angle2RegValue
-------------------------------------------------------------*/
unsigned int CServoeNeck::angle2RegValue(const double angle /* rad */)
{
	const uint16_t reg =
		1250 + (1000 / M_PI) * (angle - M_PI * 0.5);  // equation: v = s*(a-a0)
	// + v0 where s = 450/pi;
	// a0 = 0 and v0 = 750
	// cout << "Reg: " << reg << endl;
	return (reg);

	// return ( (uint16_t)( (m_MaxValue/20)*(-2*angle/M_PI+1.5) ) ); //
	// equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
	// return ( (uint16_t)( (m_MaxValue/20)*(2*angle/M_PI+1.5) ) );			//
	// equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
	// return ( (uint16_t)( (900/M_PI)*nangle + 750 ) );			// equation:
	// v
	// =
	// s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
}  // end-angle2RegValue

/*-------------------------------------------------------------
					regValue2angle
-------------------------------------------------------------*/
double CServoeNeck::regValue2angle(const uint16_t value)
{
	double angle = M_PI * 0.5 + (M_PI / 1000) * (value - 1250);
	return angle;

	// return ( -M_PI*0.5*(20*value/m_MaxValue-1.5) );					//
	// equation:
	// angle
	// =
	// (pi/2)*(20*OCR/ICR-1.5)
	// return ( -M_PI*0.25+(M_PI/1800)*(value-1050) );					//
	// equation:
	// angle
	// =
	// (pi/2)*(20*OCR/ICR-1.5)
	// return ( M_PI*0.5*(20*value/m_MaxValue-1.5) );					//
	// equation:
	// angle
	// =
	// (pi/2)*(20*OCR/ICR-1.5)
	// return ( (value-750)/(900/M_PI) );
}  // end-regValue2angle

/*-------------------------------------------------------------
					setRegisterValue
-------------------------------------------------------------*/
bool CServoeNeck::setRegisterValue(
	const uint16_t value, const uint8_t servo, bool fast)
{
	try
	{
		if (!isOpen()) return false;

		mrpt::serialization::CMessage msg, msgRx;

		// Send cmd for setting the value of the register:
		// ------------------------------------------------
		if (fast) msg.type = 0x15;
		else
			msg.type = 0x11;
		msg.content.resize(3);
		msg.content[2] = (uint8_t)value;  // Low byte
		msg.content[1] = (uint8_t)(value >> 8);	 // High byte
		msg.content[0] = servo;	 // Servo number

		archiveFrom(*this).sendMessage(msg);
		if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

		std::this_thread::sleep_for(200ms);
		return true;
	}
	catch (...)
	{
		// Error opening device:
		Close();
		return false;
	}

}  // end-setRegisterValue

/*-------------------------------------------------------------
					setRegisterValue
-------------------------------------------------------------*/
bool CServoeNeck::setRegisterValueAndSpeed(
	const uint16_t value, const uint8_t servo, const uint16_t speed)
{
	try
	{
		if (!isOpen()) return false;

		mrpt::serialization::CMessage msg, msgRx;

		// Send cmd for setting the value of the register:
		// ------------------------------------------------
		msg.type = 0x16;
		msg.content.resize(5);
		msg.content[4] = (uint8_t)speed;  // Low byte of the speed of the servo
		msg.content[3] =
			(uint8_t)(speed >> 8);	// High byte of the speed of the servo
		msg.content[2] = (uint8_t)value;  // Low byte
		msg.content[1] = (uint8_t)(value >> 8);	 // High byte
		msg.content[0] = servo;	 // Servo number

		archiveFrom(*this).sendMessage(msg);
		if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

		std::this_thread::sleep_for(200ms);
		return true;
	}
	catch (...)
	{
		// Error opening device:
		Close();
		return false;
	}

}  // end-setRegisterValue

/*-------------------------------------------------------------
					getRegisterValue
-------------------------------------------------------------*/
bool CServoeNeck::getRegisterValue(uint16_t& value, const uint8_t servo)
{
	try
	{
		if (!isOpen()) return false;

		mrpt::serialization::CMessage msg, msgRx;

		// Send cmd for obtaining the value of the OCR1A register:
		// --------------------------------------------------------
		msg.type = 0x12;
		msg.content.resize(1);
		msg.content[0] = servo;

		archiveFrom(*this).sendMessage(msg);
		if (archiveFrom(*this).receiveMessage(msgRx))
		{
			if (msgRx.content.size() != 2) return false;

			value = (msgRx.content[0] << 8) + msgRx.content[1];
			return true;
		}
		else
			return false;

		return true;
	}
	catch (...)
	{
		// Error opening device:
		Close();
		return false;
	}

}  // end-getRegisterValue

/*-------------------------------------------------------------
					getCurrentAngle
-------------------------------------------------------------*/
bool CServoeNeck::getCurrentAngle(double& angle, const uint8_t servo)
{
	uint16_t value;
	if (getRegisterValue(value, servo))
	{
		angle = regValue2angle(value);
		return true;
	}
	else
		return false;
}  // end-getCurrentAngle

/*-------------------------------------------------------------
					setAngle
-------------------------------------------------------------*/
bool CServoeNeck::setAngle(double angle, const uint8_t servo, bool fast)
{
	// double nangle = -angle;

	// if( nangle < -m_TruncateFactor*M_PI/2 )	nangle =
	// -m_TruncateFactor*M_PI/2;
	// if( nangle > m_TruncateFactor*M_PI/2 )		nangle =
	// m_TruncateFactor*M_PI/2;

	// unsigned int reg = angle2RegValue( nangle );

	// std::cout << "Angle: " << RAD2DEG( nangle ) << " - Reg: " << reg <<
	// std::endl;
	// return setRegisterValue( reg, servo );

	if (angle < -m_TruncateFactor * M_PI / 2)
		angle = -m_TruncateFactor * M_PI / 2;
	if (angle > m_TruncateFactor * M_PI / 2)
		angle = m_TruncateFactor * M_PI / 2;

	unsigned int reg = angle2RegValue(m_offsets[servo] + angle);

	return setRegisterValue(reg, servo, fast);

}  // end-getCurrentAngle

/*-------------------------------------------------------------
					setAngleAndSpeed
-------------------------------------------------------------*/
bool CServoeNeck::setAngleAndSpeed(
	double angle, const uint8_t servo, const uint8_t speed)
{
	// speed in the range 15/s-250/s
	if (angle < -m_TruncateFactor * M_PI / 2)
		angle = -m_TruncateFactor * M_PI / 2;
	if (angle > m_TruncateFactor * M_PI / 2)
		angle = m_TruncateFactor * M_PI / 2;

	unsigned int reg = angle2RegValue(m_offsets[servo] + angle);
	uint8_t thisSpeed = speed < 15 ? 15 : speed > 250 ? 250 : speed;
	auto delSpeed =
		uint16_t(0.25 * 1000000 / (500 + 1000 * (thisSpeed / 180.0f - 0.5)));
	// cout << "Speed: " << int(speed) << " -> " << delSpeed << endl;
	// std::cout << "Angle: " << RAD2DEG( angle ) << " - Reg: " << reg <<
	// std::endl;
	return setRegisterValueAndSpeed(reg, servo, delSpeed);

}  // end-getCurrentAngle

/*-------------------------------------------------------------
					setAngleWithFilter
-------------------------------------------------------------*/
bool CServoeNeck::setAngleWithFilter(
	double angle, const uint8_t servo, bool fast)
{
	double nangle = 0;
	if (m_PrevAngles.size() == m_NumPrevAngles &&
		m_NumPrevAngles != 0)  // If the deque is full populated
		m_PrevAngles.erase(
			m_PrevAngles.begin());	// Erase the first angle of the deque

	m_PrevAngles.push_back(angle);	// Push back the new angle

	std::deque<double>::iterator it;
	for (it = m_PrevAngles.begin(); it != m_PrevAngles.end();
		 ++it)	// Sum up all the elements in the deque
		nangle += *it;
	nangle /= m_PrevAngles.size();	// Mean angle

	return (setAngle(nangle, servo, fast));
}

/*-------------------------------------------------------------
					disableServo
-------------------------------------------------------------*/
bool CServoeNeck::disableServo(const uint8_t servo)
{
	try
	{
		if (!isOpen()) return false;

		mrpt::serialization::CMessage msg, msgRx;

		// Send cmd for disabling servo:
		// ----------------------------
		msg.type = 0x13;
		msg.content.resize(1);
		msg.content[0] = servo;	 // Servo number

		archiveFrom(*this).sendMessage(msg);
		if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

		return true;
	}
	catch (...)
	{
		// Error opening device:
		Close();
		return false;
	}

}  // end-getCurrentAngle

/*-------------------------------------------------------------
					enableServo
-------------------------------------------------------------*/
bool CServoeNeck::enableServo(const uint8_t servo)
{
	try
	{
		if (!isOpen()) return false;

		mrpt::serialization::CMessage msg, msgRx;

		// Send cmd for enabling the servo:
		// --------------------------------
		msg.type = 0x14;
		msg.content.resize(1);
		msg.content[0] = servo;	 // Servo number

		archiveFrom(*this).sendMessage(msg);
		if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

		return true;
	}
	catch (...)
	{
		// Error opening device:
		Close();
		return false;
	}

}  // end-getCurrentAngle

/*-------------------------------------------------------------
						Center
-------------------------------------------------------------*/
bool CServoeNeck::center(const uint8_t servo)
{
	unsigned int value = angle2RegValue(m_offsets[servo]);
	return setRegisterValue(value, servo);
}  // end-Center

/*-------------------------------------------------------------
					checkConnectionAndConnect
-------------------------------------------------------------*/
bool CServoeNeck::checkConnectionAndConnect()
{
	if (isOpen()) return true;

	try
	{
		OpenBySerialNumber(m_usbSerialNumber);
		std::this_thread::sleep_for(10ms);
		Purge();
		std::this_thread::sleep_for(10ms);
		SetLatencyTimer(1);
		SetTimeouts(300, 100);
		return true;
	}
	catch (...)
	{
		// Error opening device:
		Close();
		return false;
	}
}  // end-checkConnectionAndConnect

/*-------------------------------------------------------------
					setOffsets
-------------------------------------------------------------*/
void CServoeNeck::setOffsets(float offset0, float offset1, float offset2)
{
	m_offsets.resize(3);
	m_offsets[0] = offset0;
	m_offsets[1] = offset1;
	m_offsets[2] = offset2;
}
