/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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
