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

#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 <chrono>
#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;
}
