/* +------------------------------------------------------------------------+
   |                     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/CSerialPort.h>
#include <mrpt/hwdrivers/CPtuBase.h>
#include <mrpt/system/COutputLogger.h>

namespace mrpt::hwdrivers
{
/** This class implements initialization and communication methods to
 * control a Tilt Unit model DT-80, working in radians .
 * \ingroup mrpt_hwdrivers_grp
 */
class CTuMicos : public CPtuBase, public mrpt::system::COutputLogger
{
   public:
	/** Default constructor */

	CTuMicos() = default;

	/** Destructor */

	~CTuMicos() override { close(); }
	/*************************** Commands ***************************/

   public:
	/** Search limit forward */

	bool rangeMeasure() override;

	/** Specification of positions in absolute terms */

	bool moveToAbsPos(char axis, double nRad) override;

	/** Query position in absolute terms */

	bool absPosQ(char axis, double& nRad) override;

	/** Specify desired axis position as an offset from the current position. \n
	 *	This method recives the number of radians to move.
	 *	\code
	 *	Example of use:
	 *		TT-500 *
	 *		A *
	 *		TO * Current Tilt position is -500
	 *		TO500 *
	 *		A *
	 *		TT * Current Pan position is 1000
	 *	\endcode
	 */

	bool moveToOffPos(char axis, double nRad) override;

	/** Query position in relative terms */

	bool offPosQ(char axis, double& nRad) override;

	/** Query max movement limit of a axis in absolute terms */

	bool maxPosQ(char axis, double& nRad) override;

	/** Query min movement limit of a axis in absolute terms */

	bool minPosQ(char axis, double& nRad) override;

	/** Query if exist movement limits */

	bool enableLimitsQ(bool& enable) override;	// Query if exist some limit

	/** Enable/Disable movement limits */

	bool enableLimits(bool set) override;

	/** With I mode (default) instructs pan-tilt unit to immediately
	 *	execute positional commands. \n
	 *	In S mode instructs pan-tilt unit to execute positional commands
	 *	only when an Await Position Command Completion command is executed
	 *	or when put into Immediate Execution Mode. \n
	 *	\code
	 *	Example of use of S mode:
	 *		DR *
	 *		S *
	 *		PP1500 *
	 *		TP-900 *
	 *		PP * Current Pan position is 0
	 *		TP * Current Tilt position is 0
	 *		A *
	 *		PP * Current Pan position is 1500
	 *		TP * Current Tilt position is -900
	 *	\endcode
	 */

	bool inmediateExecution(bool set) override;

	/** Wait the finish of the last position command to
	 *	continue accept commands
	 */

	bool aWait() override;

	/** Inmediately stop all */

	bool haltAll() override;

	/** Inmediately stop */

	bool halt(char axis) override;

	/** Specification of turn speed */

	bool speed(char axis, double radSec) override;

	/** Query turn speed */

	bool speedQ(char axis, double& radSec) override;

	/** Specification (de/a)celeration in turn */

	bool aceleration(char axis, double radSec2) override;

	/** Query (de/a)celeration in turn */

	bool acelerationQ(char axis, double& radSec2) override;

	/** Specification of velocity to which start and finish
	 *	the (de/a)celeration
	 */

	bool baseSpeed(char axis, double radSec) override;

	/** Query velocity to which start and finish
	 *	the (de/a)celeration
	 */

	bool baseSpeedQ(char axis, double& radSec) override;

	/** Specification of velocity upper limit */

	bool upperSpeed(char axis, double radSec) override;

	/** Query velocity upper limit */

	bool upperSpeedQ(char axis, double& radSec) override;

	/** Specification of velocity lower limit */

	bool lowerSpeed(char axis, double radSec) override;

	/** Query velocity lower limit */

	bool lowerSpeedQ(char axis, double& radSec) override;

	/** Reset PTU to initial state */

	bool reset() override;

	/** Save or restart default values */

	bool save() override;

	/** Restore default values */

	bool restoreDefaults() override;

	/** Restore factory default values */

	bool restoreFactoryDefaults() override;

	/** Version and CopyRights */

	bool version(char* nVersion) override;

	/** Number of version */

	void nversion(double& nVersion) override;

	/** Query power mode */

	bool powerModeQ(bool transit, char& mode) override;

	/** Specification of power mode */

	bool powerMode(bool transit, char mode) override;

	/** Clear controller internal stack */

	bool clear();

	/** Set limits of movement */

	bool setLimits(char axis, double& l, double& u) override;

	/* Change motion direction */

	bool changeMotionDir() override;

	/**************************** State Queries ********************/

	int checkErrors() override;

	/** Clear errors **/

	void clearErrors() override {}
	/*************************** Other member methods *****************/

   public:
	/** PTU and serial port initialization */

	bool init(const std::string& port) override;

	/** Close Connection with serial port */

	void close() override;

	/** To obtains the mistake for use discrete values when the movement
	 *	is expressed in radians. Parameters are the absolute position in
	 *	radians and the axis desired
	 */

	double radError(char axis, double nRadMoved) override;

	/**  To obtain the discrete value for a number of radians */

	long radToPos(char axis, double nRad) override;

	/** To obtain the number of radians for a discrete value */

	double posToRad(char axis, long nPos) override;

	/** Performs a scan in the axis indicated and whit the precision desired. \n
	 *		\param <axis> {Pan or Till} \n
	 *		\param <tWait> {Wait time betwen commands} \n
	 *		\param <initial> {initial position}
	 *		\param <final> {final position}
	 *		\param <radPre> {radians precision for the scan}
	 */

	bool scan(char axis, int wait, float initial, float final, double radPre)
		override;

	/** Query verbose mode */

	bool verboseQ(bool& modo) override;

	/** Set verbose. \n
	 *	\conde
	 *	Example of response with FV (verbose) active:
	 *		FV *
	 *		PP * Current pan position is 0
	 *		Example of response with FT (terse) active:
	 *		FT *
	 *		PP * 0
	 *	\endcode
	 */

	bool verbose(bool set) override;

	/** Query echo mode */

	bool echoModeQ(bool& mode) override;

	/** Enable/Disable echo response with command. \n
	 *	\code
	 *	Example of use (EE supposed):
	 *		PP * 22
	 *		ED *
	 *		<pp entered again, but not echoed>* 22
	 *	\endcode
	 */

	bool echoMode(bool mode) override;

	/** Query the pan and tilt resolution per position moved
	 *	and initialize local atributes
	 */

	bool resolution() override;

	/** Check if ptu is moving */

	double status(double& rad) override;

	/*************************** Methods for internal use ****************/

   private:
	/** To transmition commands to the PTU */

	bool transmit(const char* command) override;

	/** To receive the responseof the PTU */

	bool receive(const char* command, char* response) override;

	/** Used to obtains a number of radians */

	bool radQuerry(char axis, char command, double& nRad) override;

	/** Method used for asign a number of radians with a command */

	bool radAsign(char axis, char command, double nRad) override;

	/** Convert string to double */

	static double convertToDouble(char* sDouble);

	/** Convert string to long */

	static long convertToLong(char* sLong);

	/**************************** Atributes ********************/

   public:
	/* Index of the Tilt axis in use */

	int axis_index;

};	// End of class

}  // namespace mrpt::hwdrivers
