/** \file
 *
 *  Contains the EZServoServo class declaration.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef EZSERVOSERVO_H_
#define EZSERVOSERVO_H_

#include "component/SyncComponent.h"
#include "io/LoadControl.h"
#include "io/UartStream.h"

/**
 *  This is the parent class for device drivers that interface with EZServos
 *  Since all EZServos shares the same RS-485 bus, this class has a static instance of
 *  the StdUart class to interact with the bus.
 *
 *  \ingroup modules_controller
 */
class EZServoServo : public SyncServoComponent
{
public:
    // EZServo Servos
    typedef enum
    {
        BUOYANCY,
        ELEVATOR,
        MASS_SHIFTER,
        RUDDER,
        THRUSTER,
        DOCKING,
        NUM_EZSERVO_SERVOS
    } EZServoServoType;

    EZServoServo( const Str& name, const ConfigURI& uartCfg, const ConfigURI& baudCfg,
                  const Module* module, EZServoServoType motorServoType,
                  int divisor = 1, int modulo = 0 );
    virtual ~EZServoServo();

protected:

    // Serial port
    UartStream uart_;

    // Call at start of initialize
    void initializeStart( void );

    // Call at start of uninitialize
    void uninitializeStart( void );

    // Call at end of uninitialize
    void uninitializeEnd();

    int controlAddress_;
    EZServoServoType motorServoType_;

    // The error set returned by the EZ Servo
    typedef enum
    {
        NO_ERROR = 0x0,
        INIT_ERROR = 0x1,
        BAD_COMMAND_ERROR = 0x2,
        BAD_OPERAND_ERROR = 0x3,
        COMM_ERROR = 0x5,
        NOT_INIT_ERROR = 0x7,
        OVERLOAD_ERROR = 0x9,
        NOT_ALLOWED_ERROR = 0xB,
        COMMAND_OVERFLOW_ERROR = 0xF
    } EZServoError;

    // Most recent ez servo error;
    EZServoError ezServoError_;

    // Timeouts
    Timespan eepromWriteTimeout_;
    Timestamp startTime_;

    char uartResponse_[30];

    // The status bitmask
    static const int RECEIVE_READY = 0x2; // Note: bit 6 is always set. Ready bit is bit 5.

    // returns absolute position of motor. Checks position against low/high limits
    float getPosition();

    // returns the value of ADC 4,3,2,or 1
    int getADC( int channel );

    // Returns nan if there is no/invalid response.
    // Otherwise returns the value from the command
    // send to the EZ Servo
    float isCommunicating( char *cmd );

    // returns the current encoder position
    float getVelocityCmd();

    // Verify that motor controller position is within physical limits
    bool checkPosition( float position );

    // Looks at EZServo status and sets errors as applicable.
    // If checkForReady is true, returns true if controller is reporting
    // ready and false if not.
    virtual bool checkResponse( bool checkForReady );

    // Limits the value to the range limtLo_+1 to limitHi_-1
    float limit( float value );

    // Reads configuration settings
    virtual bool readConfig();

    // Configuration settings
    float powerOnTimeoutCfg_;  // Time to allow system to power up before commanding
    float powerOffTimeoutCfg_; // Time to allow system to fully power down before restarting
    float currLimitCfg_;       // Percent of current allowed
    float limitHiCfg_;         // High physical limit for motor controller
    float limitLoCfg_;         // Low physical limit for motor controller
    float pidWCfg_;            // Proportional gain
    float pidXCfg_;            // Integral gain
    float pidYCfg_;            // Differential gain
    float overloadTimeoutCfg_; // Timeout to wait before throwing overload error
    float accelCfg_;           // Encoder ticks / 32.768 per second squared
    float velocityCfg_;        // Encoder ticks / 32.768 per second
    float offsetAngleCfg_;
    float countsPerDegCfg_;    //motor "ticks" per degree of control surface motion
    float mtrCenterCfg_;       // 0 degrees "centered" control surface
    float deviationAngleCfg_;  // Number of degrees deviation allowed between expected and actual

    /// Config settings shared by all Servos:
    ConfigReader* powerOnTimeoutCfgReader_;  // Time to allow system to power up before commanding
    ConfigReader* powerOffTimeoutCfgReader_; // Time to allow system to fully power down before restarting
    ConfigReader* currLimitCfgReader_;       // Percent of current allowed

    // Config settings shared by all except Thruster:
    ConfigReader* limitHiCfgReader_;         // High physical limit for motor controller
    ConfigReader* limitLoCfgReader_;         // Low physical limit for motor controller

    // Config settings shared by all except Mass:
    ConfigReader* pidWCfgReader_;            // Proportional gain
    ConfigReader* pidXCfgReader_;            // Integral gain
    ConfigReader* pidYCfgReader_;            // Differential gain

    // Config settings shared by all except Elevator + Rudder:
    ConfigReader* overloadTimeoutCfgReader_; // Timeout to wait before throwing overload error
    ConfigReader* accelCfgReader_;           // Encoder ticks / 32.768 per second squared

    // Config settings shared by Mass + Buoyancy:
    ConfigReader* velocityCfgReader_;        // Encoder ticks / 32.768 per second

    // Config settings shared by Elevator + Rudder:
    ConfigReader* offsetAngleCfgReader_;
    ConfigReader* countsPerDegCfgReader_;   //motor "ticks" per degree of control surface motion
    ConfigReader* mtrCenterCfgReader_;      // 0 degrees "centered" control surface
    ConfigReader* deviationAngleCfgReader_; // Number of degrees deviation allowed between expected and actual

private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    EZServoServo( const EZServoServo& old ); // disallow copy constructor

};

#endif /*EZSERVOSERVO_H_*/