/** \file
 *
 *  Contains the DockingServo class declaration.
 *
 *  DockingServo.h should only be included by DockingServo.cpp
 *  Other classes should include DockingServoIF.h
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef DOCKINGSERVO_H_
#define DOCKINGSERVO_H_

#include "dockModule/DockIF.h"
#include "EZServoServo.h"
#include "io/LoadControl.h"
#include "io/UartStream.h"


/**
 *  This is the component interface for the EZServo that controls the vehicle's
 *  docking arm.  Since it shares the same RS-485 bus with the other EZServos,
 *  it is a child class of EZServoServo, which has a static instance of
 *  the StdUart class to interact with the bus.
 *
 *  DockingServo.h should only be included by DockingServo.cpp
 *  Other classes should include DockingServoIF.h
 *
 *  \ingroup modules_controller
 */
class DockingServo : public EZServoServo
{
public:
    DockingServo( const Module* module );
    virtual ~DockingServo();

    virtual void run();

    /// Do what needs to be done to run
    /// Similar to initialize, in old init/run/uninit sequence
    virtual RunState start();

    /// Might follow a STOP...START sequence
    virtual RunState starting();

    /// Pause for a short period (indicated by pauseTime)
    virtual RunState pause();

    /// Should eventually follow a PAUSE request: should set continueTime
    virtual RunState paused();

    /// Resume from PAUSE
    virtual RunState resume();

    /// Might follow a PAUSE...RESUME sequence
    virtual RunState resuming();

    /// Should eventually follow a START request or RESETTING
    virtual RunState runnable();

    /// Might occur in case of Error
    virtual RunState resetting()
    {
        return stop();
    }

    /// Initial state -- can be later followed by START
    virtual RunState stop();

    virtual RunState stopping();

    /// Initial state -- can be later followed by START
    virtual RunState stopped();

    // Shuts down motor controllers
    void uninitialize( void );

    /// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
    virtual ConfigURI getConfigURI( ConfigOption configOption ) const;

private:

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

    // Instance of load controller
    LoadControl loadControl_;

    // The outputs themselves


    // Slate inputs
    DataReader* armAngleReader_;
    DataReader* modeCmdReader_;

    // Slate outputs
    DataWriter* armAngleWriter_;
    DataWriter* cablePresentWriter_;
    DataWriter* cableValueWriter_;
    DataWriter* modeWriter_;

    // returns true if data is requested from one of the readers
    bool isDataRequested();

    // returns true if a new position has been commanded
    bool isNeeded();

    typedef enum
    {
        INITIALIZE,   // Program basic initialization into EEPROM
        WAIT,         // Wait for EEPROM write to finish
        HOME,         // Do any centering/homing, etc if needed
        EXECUTE,      // Execute any stored programs
        VERIFY,       // Confirm operable and no errors
        DONE,         // Ready to run
    } StartupSequence;

    StartupSequence startup_;

    // indicates whether things are ok to run
    bool ok_;

    // Actual and commanded angles
    float actualArmAngle_;
    bool haveActualAngle_;
    float cmdArmAngle_;
    float openAngle_;
    float closedAngle_;

    // Debugging outputs
    bool debug_;

    // read vehicle config variables
    bool readConfig( void );

    ConfigReader* openAngleCfgReader_;
    ConfigReader* closedAngleCfgReader_;

    // the given motor controller is ready to receive commands
    bool armRdy_;

    int lastMode_;      // Store the last known commanded mode
    bool modeChanged_;  // Lets us know if the mode has changed this cycle to allow command change mid transit
    bool armOpen_;      // True means ready to dock
    bool cablePresent_; // True is cable present
    int cableStatus_;   // holds the actual value read from the sensor

    DockIF::DockingState mode_, modeCmd_; // modeCmd_ is the commanded mode while mode_ is what mode the servo is actually in

    // Initialization routines. Returns true if initialization completed without error
    bool initServo( void );
    void publishData( void );
    void logCableStatus( void );
    bool getSimulatedMeasurements( void ); // TODO fix simulator where appropriate so that this method just reads sim outputs and rewrites them to the slate in the appropriate places

};

#endif /*DockingServo_H_*/