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

#include "ThrusterModel.h"

#include "utils/AuvMath.h"

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++ ThrusterModel ++++++++++++++++++++++++++++++++++++++++++++++++++++++

ThrusterModel::ThrusterModel()
    : prevThrust_( 0.0 ),
      prevTorque_( 0.0 ),
      currentPropOmega_( 0.0 ),
      kWash_( 0.0 ),
      kThrust_( 0.0 ),
      kTorque_( 0.0 )
{}

void ThrusterModel::initialize( float designSpeed, float designPropEff, float designOmega, float designThrust, float designTorque )
{
    float designWash = designSpeed / designPropEff;
    kWash_ = designWash / designOmega;
    kThrust_ = designThrust / ( designWash * ( designWash - designSpeed ) );
    kTorque_ = designTorque / ( designWash * ( designWash - designSpeed ) );
}

/// Unlike the actuator model, the thruster model is very simple, with
/// no time delay.  Rather, if asked for behavior at dt=0, will give
/// previous results.  If asked for results at dt>0 will give new
/// calculations
void ThrusterModel::project( float propOmega, float u, float dt, float &thrust, float &torque )
{
    if( dt == 0.0 )
    {
        thrust = prevThrust_;
        torque = prevTorque_;
    }
    else
    {
        thrust = thrusterForce( propOmega, u );
        torque = thrusterTorque( propOmega, u );
    }
}

void ThrusterModel::advance( float propOmega, float u, float dt )
{
    prevThrust_ = thrusterForce( propOmega, u );
    prevTorque_ = thrusterTorque( propOmega, u );

    currentPropOmega_ = propOmega;
}


/* Thruster_force
 * Now accounts for u, and will even correctly predict a negative thrust if
 * the vehicle is going faster than the prop.
 * In this model, wash is the speed of the outflow from the prop, in
 * vehicle coordinates.
 */
float ThrusterModel::thrusterForce( float omegaP, float u )
{
    float wash = kWash_ * omegaP;
    return kThrust_ * wash * ( wash - u );
}

/* Thruster_torque
 * Now accounts for u, and will even correctly predict a negative torque if
 * the vehicle is going faster than the prop.
 * In this model, wash is the speed of the outflow from the prop, in
 * vehicle coordinates.
 */
float ThrusterModel::thrusterTorque( float omegaP, float u )
{
    float wash = kWash_ * omegaP;
    return kTorque_ * wash * ( wash - u );
}