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

#ifndef ACTUATORMODEL_H_
#define ACTUATORMODEL_H_

#include "utils/AuvMath.h"

/**
 *  Encapsulates the actuator model.
 *
 *  The Ody/Altex simulator has a relatively complex actuator model.  It
 *  models time delays/maximum rates for the Ody motors and includes a
 *  small hysteresis (to model slop and backlash?).   This class encapsulates
 *  those calculations to make the main simulator code just a little
 *  more modular.
 *
 *  The code is complicated slightly by the fact the Runge-Kutta
 *  integration evaluates at three different time steps (t+0, t+smallDt/2, and t+smallDt)
 *  Since they're monotonically increasing we could advance the concept of
 *  'current' each time we, but instead I've written it to take
 *  (0, smallDt/2, smallDt) as a parameter, each time just "projecting" into the future
 *  without updating its state.  Then an "advance" function is called separately
 *  to cause it to update its state by smallDt.
 *
 *  May look goofy, but everything here is specified in centiseconds
 *  since we're using a 100-depth delay buffer but I want it to span 1
 *  second.  It doesn't make any sense to use a 1000-long buffer for this
 *
 *  \ingroup modules_simulator
 */
class ActuatorModel
{
public:

    /// Construct an model using the given params.
    ActuatorModel();

    /// Destructor
    ~ActuatorModel()
    {
        ;
    };

    void setCommand( const float command );

    // Predict the actuator position at smallDt
    // ("look forward by a time step")
    float project( const float dt );

    // Predict the actuator position at dt and store it as "previous"
    // ("take a time step")
    float advance( const float dt, bool debug = false );

    /// \param width The width of the hysteresis
    /// \param center The center of the hysteresis curve (normally 0)
    void setHyst( const float width, const float center )
    {
        hystWidth_ = width;
        hystCenter_ = center;
    }

    void setSpeed( const float speed )
    {
        speed_ = speed;
    }

    void setPrevious( const float previous );

    float getCurrent( void )
    {
        return prevPosition_;
    }

private:

    // Predict the actuator position at smallDt
    // ("look forward by a time step")
    void project( const float dt, float &shaft, float& position, bool debug = false );

    float next( const float dt, bool debug = false );

    float hysteresis( const float shaft );

    float command_;

    float prevPosition_, prevShaft_;

    bool movingToLeft_, movingToRight_;

    /// The width in of the hysteresis
    float hystWidth_;

    /// The center of the hysteresis curve (normally 0)
    float hystCenter_;

    /// Speed of the acutator (units/sec)
    float speed_;

};

#endif /*ACTUATORMODEL_H_*/