/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/comms/CInterfaceFTDI.h>
#include <mrpt/core/exceptions.h>

namespace mrpt::hwdrivers
{
/** A USB-interface for a custom "robotic neck" designed at MAPIR lab.
 * \ingroup mrpt_hwdrivers_grp */
class CServoeNeck : public mrpt::comms::CInterfaceFTDI
{
   public:
	CServoeNeck();
	~CServoeNeck() override;

	/** Gets the firmware version of the eNeck board.
	 * \param out_firmwareVersion: [OUTPUT] A string containing the firmware
	 * version.
	 * \return Whether or not the procedure succeded.
	 */
	bool queryFirmwareVersion(std::string& out_firmwareVersion);

	/** Gets the current angle of the servo (in radians within (-pi,pi))
	 * \param Angle: [OUT] The current angle.
	 * \param Servo: [IN] The id of the servo (in our ATMEGA16, from 0 to 2).
	 * \return Whether or not the procedure succeded.
	 */
	bool getCurrentAngle(double& angle, const uint8_t servo = 0);

	/** Turns the servo up to the specified angle (in radians in the range
	 * -pi,pi, other values will be saturated to the maximum or the mininum)
	 * \param Angle: the desired angle to turn.
	 * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
	 * 2).
	 * \param Fast: indicates if the servo must reach the angle at maximum
	 * speed
	 * \return Whether or not the procedure succeded.
	 */
	bool setAngle(double angle, const uint8_t servo = 0, bool fast = false);

	/** Turns the servo up to the specified angle (in radians in the range
	 * -pi,pi, other values will be saturated to the maximum or the mininum)
	 * \param Angle: the desired angle to turn.
	 * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
	 * 2).
	 * \param Speed: indicates the speed of the servo
	 * \return Whether or not the procedure succeded.
	 */
	bool setAngleAndSpeed(
		double angle, const uint8_t servo, const uint8_t speed);

	/** Turns the servo up to the specified angle (in radians in the range
	 * -pi,pi) filtered by average with the last N specified angles.
	 * \param Angle: the new desired angle to turn.
	 * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
	 * 2).
	 * \param Fast: indicates if the servo must reach the angle at maximum
	 * speed
	 * \return Whether or not the procedure succeded.
	 */
	bool setAngleWithFilter(
		double angle, const uint8_t servo = 0, bool fast = false);

	/** Disables the servo so the neck will be loose.
	 * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
	 * 2).
	 * \return Whether or not the procedure succeded.
	 */
	bool disableServo(const uint8_t servo = 0);

	/** Enables the servo so the neck will be tight.
	 * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
	 * 2).
	 * \return Whether or not the procedure succeded.
	 */
	bool enableServo(const uint8_t servo = 0);

	/** Centers the servo at zero position
	 */
	bool center(const uint8_t servo = 0);

	/** Gets the truncate factor of the turn
	 */
	double getTruncateFactor() { return m_TruncateFactor; }
	/** Gets the truncate factor of the turn
	 */
	void setTruncateFactor(const double factor)
	{
		ASSERT_(factor > 0 && factor < 1);
		m_TruncateFactor = factor;
	}

	/** Gets the truncate factor of the turn
	 */
	void setNumberOfPreviousAngles(const unsigned int number)
	{
		m_NumPrevAngles = number;
	}

	/** Gets the truncate factor of the turn
	 */
	unsigned int getNumberOfPreviousAngles() { return m_NumPrevAngles; }
	/**Load the Offset values for each servo
	 */
	void setOffsets(float offset0, float offset1, float offset2);

   protected:
	/** A copy of the device serial number (to open the USB FTDI chip). */
	std::string m_usbSerialNumber;
	/** The value set in the ICR register within the ATMEGA16 controller. */
	double m_MaxValue{10000};
	/** The range of turn of the servo will be truncated to
	 * "+-m_truncate_factor*(pi/2)". */
	double m_TruncateFactor{0.5};
	/** A vector containing the last N angles which where passed to the servo
	 * (for averaging) */
	std::deque<double> m_PrevAngles;
	/** Number of previous angles to store for averaging */
	unsigned int m_NumPrevAngles{5};
	/** The offset used for each servo */
	std::vector<float> m_offsets;

	bool setRegisterValue(
		const uint16_t value, const uint8_t servo = 0, bool fast = false);
	bool setRegisterValueAndSpeed(
		const uint16_t value, const uint8_t servo, const uint16_t speed);
	bool getRegisterValue(uint16_t& value, const uint8_t servo = 0);

   private:
	/** Converts from a decimal angle (in radians) to the corresponding register
	 * value for the ATMEGA16 controller (for inner use only).
	 * \param The angle to convert.
	 * \return The value of the register to send.
	 */
	unsigned int angle2RegValue(const double angle);  // Angle in rad

	/** Converts from a certain value of the ATMEGA16 PWM register to the
	 * corresponding decimal angle (for inner use only).
	 * \param The value to convert.
	 * \return The corresponding angle.
	 */
	double regValue2angle(const uint16_t value);

	/** Tries to connect to the USB device (if disconnected).
	 * \return True on connection OK, false on error.
	 */
	bool checkConnectionAndConnect();

};	// End of class

}  // namespace mrpt::hwdrivers
