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

#include "ActuatorModel.h"

#include <math.h>
#include <stdio.h>

/// Initialize this actuator model.  Any sort of actuator-specific params could
/// be passed in here.  Right now its just info on the hysteresis/backlash
/// in each actuator.
///
ActuatorModel::ActuatorModel()
    : command_( 0.0 ),
      prevPosition_( 0.0 ),
      prevShaft_( 0.0 ),
      movingToLeft_( false ),
      movingToRight_( false ),
      hystWidth_( 0.0 ),
      hystCenter_( 0.0 ),
      speed_( 0.0 )
{}

void ActuatorModel::setCommand( const float command )
{
    command_ = command;
}

float ActuatorModel::project( const float dt )
{
    //if ( hystWidth_ > 0 ) printf("projectDt=%g, ", dt);
    float shaft;
    float position;

    project( dt, shaft, position );

    return position;
}

// Predict the actuator position at dt and store it as "previous"
// ("take a time step")
float ActuatorModel::advance( const float dt, bool debug )
{
    //if ( hystWidth_ > 0 ) printf("advanceDt=%g, ", dt);
    float shaft = prevShaft_;
    project( dt, shaft, prevPosition_, debug );
    prevShaft_ = shaft;
    return prevPosition_;
}

/// Override the model to set the "current" value
void ActuatorModel::setPrevious( const float previous )
{
    prevShaft_ = previous;
    prevPosition_ = previous;
}

// returns position and shaft position for advance()
void ActuatorModel::project( const float dt, float &shaft, float& position, bool debug )
{
    shaft = next( dt, debug );
    //if ( hystWidth_ > 0 ) printf( "hyteresis( shaft%f), prevShaft_%f, prevPosition_%f, movingToLeft_%d, movingToRight_%d, hystWidth_%f, hystCenter_%f )=", shaft, prevShaft_, prevPosition_, movingToLeft_, movingToRight_, hystWidth_, hystCenter_ );
    position = hysteresis( shaft );
    //if ( hystWidth_ > 0 ) printf( "%f\n", position );
}

float ActuatorModel::next( const float dt, bool debug )
{
    float newPosition( command_ );
    /* how far fin can go in smallDt seconds */
    float canMoveInDt( speed_ * dt );
    if( debug ) printf( "dt=%g, canMoveInDt=%g\n", dt, canMoveInDt );

    if( fabs( prevShaft_ - command_ ) > canMoveInDt )
    {
        if( prevShaft_ > command_ )
            newPosition = prevShaft_ - canMoveInDt;
        if( prevShaft_ < command_ )
            newPosition = prevShaft_ + canMoveInDt;
    }
    return ( newPosition );
}

float ActuatorModel::hysteresis( const float shaft )
{
    float dShaft( shaft - prevShaft_ );
    float position( prevPosition_ );

    if( dShaft > 0 &&
            ( movingToRight_ || shaft - prevPosition_ > hystCenter_ + hystWidth_ / 2.0 ) )
    {
        movingToRight_ = true;
        position += dShaft;
    }
    else
    {
        movingToRight_ = false;
    }
    if( dShaft < 0 &&
            ( movingToLeft_ || shaft - prevPosition_ < hystCenter_ - hystWidth_ / 2.0 ) )
    {
        movingToLeft_ = true;
        position += dShaft;
    }
    else
    {
        movingToLeft_ = false;
    }
    return position;
}