#ifndef _ROBOTEQ_H_
#define _ROBOTEQ_H_

#include "../include/serial.h"

#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <errno.h>

using namespace std;

#define TRUE				0
#define FALSE				-1
#define DIRECTION_FORWARD	1
#define DIRECTION_REVERSE	2
#define ACCESSORY_C			1
#define ACCESSORY_D			2
#define BATTERY_MAIN		1
#define BATTERY_INTERNAL	2
#define DIGITAL_E			1
#define DIGITAL_F			2
#define DIGITAL_ESTOP		3
#define OFF					1
#define ON					2

/** 
 * This structure is a holder for the motor parameters
 */
struct motorParams {

	// This structure can be used to read or update these parameters
	int speed1;	/**< speed of motor 1 */
	int speed2;	/**< speed of motor 2 */
	int dir1;	/**< direction of motor 1 */
	int dir2;	/**< direction of motor 2 */
	int power1;	/**< returned power level of motor 1 */
	int power2;	/**< returned power level of motor 2 */
	int amps1;	/**< amperage drawn by motor 1 */
	int amps2;	/**< amperage drawn by motor 2 */
};

/** 
 * This structure is a holder for the analog input parameters
 */
struct analogParams {
	
	// This structure can be used to read the analog voltages
	int val1;	/**< Value, speed, or position for input 1 */
	int val2;	/**< Value, speed, or position for input 2 */
};

/** 
 * This structure is a holder for the heat readings
 */
struct heatParams {
	
	// This structure can be used to read the temperature of the heatsinks
	int val1;	/**< Temperature of heatsink 1 */
	int val2;	/**< Temperature of heatsink 2 */
};

/** 
 * This structure is a holder for the battery voltages
 */
struct batteryParams {
	
	// This structure can be used to read the voltage of the batteries
	int val1;	/**< Voltage of main battery */
	int val2;	/**< Voltage of internal 12v regulator */
};

/** 
 * This structure is a holder for the digital inputs
 */
struct digitalParams {
	
	// This structure can be used to read state of the digital inputs
	int val1;	/**< Value of input 1 */
	int val2;	/**< Value of input 2 */
};

/**
 * This class is a serial port wrapper for the Roboteq motor controller
 */
class Roboteq {
	public:

		/**
		 * Constructor.
		 * Sets power-on defaults.
		 */
		Roboteq();

		/** 
		 * Deconstructor.
		 * Ensures sytem is shut down and that the serial port is properly closed.
		 */
		~Roboteq();

		/**
		 * Roboteq initialization.
		 * Connects to the serial port and ensures Roboteq is in RS-232 mode and ready
 		 * @return int - TRUE if successful, FALSE if error
		 */
		int connect();

		/**
		 * Sets motor speed value.
		 * Sends the proper command to the serial port to change speed
		 * @param channel - the motor channel to modify (either 1 or 2)
		 * @param direction - the direction to turn (either DIRECTION_FORWARD or DIRECTION_REVERSE)
		 * @param speed - value to assign to motor (must be 0 - 127)		
		 * @return int - TRUE if successful, FALSE if error
		 */
		int setSpeed(int channel, int direction, int speed);

		/**
		 * Sets motor speed values for both motors.
		 * Sends the proper commands to the serial port to change speeds
		 * @param params - motorParams struct with properly assigned member varibles
		 * @return int - TRUE if successful, FALSE if error
		 */
		int setSpeed(motorParams* params);

		/**
		 * Controls the accesory ports C and D.
		 * Port C is a 5-24v, 2 Amp switch. Port D is low current (not availabe on AX3500)
		 * @param port - the port to control (either ACCESSORY_C or ACCESSORY_D)
		 * @param state - the state to set (either ON or OFF)
		 * @return int - TRUE if successful, FALSE if error
		 */
		int setAccessory(int port, int state);

		/**
		 * Gets a motor speed value.
		 * Queries the Roboteq for an actual motor speed.
		 * @param channel - the motor channel to read (either 1 or 2)
		 * @return int - motor speed (0 to 127)
		 */
		int getSpeed(int channel);

		/**
		 * Gets both motor speed values.
		 * Queries the Roboteq for the actual motor speeds.  Must use motorParams structure.
		 * @param params - motorParams struct to fill with current speeds
		 * @return int - TRUE if successful, FALSE if error
		 */
		int getSpeed(motorParams* params);

		/**
		 * Gets analog value for one input.
		 * Queries the Roboteq for the value, speed, or position for one of the analog inputs.  (Roboteq operating mode determines the datatype)
		 * @param channel - analog channel to be read (either 1 or 2)
		 * @return int - analog value if successful, FALSE if error
		 */
		int getAnalog(int channel);

		/**
		 * Gets analog values.
		 * Queries the Roboteq for the value, speed, or position for both of the analog inputs.  (Roboteq operating mode determines the datatype)
		 * @param params - analogParams struct to hold the values
		 * @return int - TRUE if successful, FALSE if error
		 */
		int getAnalog(analogParams* params);

		/**
		 * Get heatsink temperature.
		 * Queries the Roboteq for the temperature reading from one of the thermisters. (See lookup table in Roboteq documentation for equations to convert to actual temperatures)
		 * @param channel - temperature channel to read (either 1 or 2)
		 * @return int	- temperature value if successful, FALSE if error
		 * @todo Add helper fuction to do lookup table conversion
		 */
		int getTemp(int channel);

		/**
		 * Get heatsink temperatures.
		 * Queries the Roboteq for the temperature reading from both of the thermisters. (See lookup table in Roboteq documentation for equations to convert to actual temperatures)
		 * @param params - heatParams struct to hold temperature readings
		 * @return int	- TRUE if successful, FALSE if error
		 * @todo Add helper fuction to do lookup table conversion
		 */
		int getTemp(heatParams params);

		/**
		 * Get a battery voltage.
		 * Queries the Roboteq for the voltage of either the main battery or the internal 12v regulator. (See lookup table in Roboteq documentation for equations to convert to actual voltages)
		 * @param battery - battery voltage to read (either BATTERY_MAIN or BATTERY_INTERNAL)
		 * @return int - battery value if successful, FALSE if error
		 * @todo Add helper fuction to do lookup table conversion
		 */
		int getBattery(int battery);

		/**
		 * Get battery voltages.
		 * Queries the Roboteq for the voltage of both the main battery and the internal 12v regulator. (See lookup table in Roboteq documentation for equations to convert to actual voltages)
		 * @param params - batteryParams struct to hold voltage readings
		 * @return int - TRUE if successful, FALSE if error
		 * @todo Add helper fuction to do lookup table conversion
		 */
		int getBattery(batteryParams params);

		/** 
		 * Get digital inputs.
		 * Queries the Roboteq for the status of a specific digital input.
		 * @param channel - the channel to query (DIGITAL_E, DIGITAL_F, or DIGITAL_ESTOP)
		 * @return int - value of specified input if successful, FALSE if error
		 */
		int getDigital(int channel);

		/** 
		 * Get digital inputs.
		 * Queries the Roboteq for the status of all specific digital inputs.
		 * @param params - digitalParams struct to hold the status of each input
		 * @return int - TRUE if successful, FALSE if error
		 */
		int getDigital(digitalParams params);

		/**
		 * Advanced settings.
		 * Sets one of the configuration settings on the Roboteq.  See the Roboteq manual for details on each of the available paramters.
		 * @param param - the value of the parameter to modify
		 * @param val - the value to assign
		 * @return int - TRUE if successful, FALSE if error
		 */
		int setParam(int param, int val);
		
		/**
		 * Advanced settings.
		 * Gets one of the configuration settings on the Roboteq.  See the Roboteq manual for details on each of the available paramters.
		 * @param param - the value of the parameter to read
		 * @return int - value if successful, FALSE if error
		 */
		int getParam(int param);

		/**
		 * Apply settings to flash
		 * Changes to the RAM parameters must be saved to flash to take effect.  This allows updates of system parameters without a reset.
		 * @return int - TRUE if successful, FALSE if error
		 */
		int writeParams();

		/**
		 * RS232 Control Mode
		 * Switches the Roboteq into RS232 control mode.
		 * @return int - TRUE if successful, FALSE if error
		 */
		int setRS232();

		/**
		 * Reset controller.
		 * Sends the reset message to the Roboteq.  The init() method should probably be called again after this.
		 * @return int - TRUE if successful, FALSE if error
		 */
		int reset();

		/** 
		 * Motors disabled and serial port connection closed.
		 */
		void disconnect();

		char* dev;	/**< Serial port device */

	private:

		motorParams defaultMotorParams; /**< Initial power-on settings */
		Serial port;			/**< Serial port used for Roboteq*/

		/**
		 * Appends a newline character to the end of a serial command.
		 */
		void newLine();
		
};

#endif //_ROBOTEQ_H