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

#include <math.h>
#include <cstdio>

#include "Trajectory.h"
#include "utils/AuvMath.h"

// Trajectory Constructor
Trajectory::Trajectory( bool periodic )
    : initialized_( false ),
      x_( 0 ),
      xDot_( 0 ),
      periodic_( periodic )
{}

Trajectory::~Trajectory()
{}

///  project: executes the trajectory generator for one time step
///  inputs:
///      xf is the final, or goal in radians
///      xdotmax is the max desired rate (foo/sec)
///      xdot2 is the acceleration rate (foo/sec2)
///  outputs:
///      the trajectory structure will have updated x and xdot elements
float Trajectory::project( float x0, float dt, float xf, float xdotmax, float xAccel, bool debug )
{
    if( !initialized_ )
    {
        // Check for nan or divide-by-zero
        if( ( x0 != x0 ) )
        {
            return x0;
        }
        x_ = x0;
        xDot_ = 0.0;
        initialized_ = true;
    }

    float dx;
    if( periodic_ )
        dx = AuvMath::ModPi( xf - x_ );
    else
        dx = xf - x_;

    float decelDx = xDot_ * xDot_ / 2 / xAccel;

    if( dx > 0 )
    {
        if( dx > decelDx || xDot_ < 0 )
        {
            if( debug ) printf( "1a" );
            xDot_ += xAccel * dt;
            if( xDot_ * dt > dx )
            {
                xDot_ = dx / dt;
                if( debug ) printf( "x" );
            }
        }
        else
        {
            if( debug ) printf( "1b" );
            xDot_ -= xAccel * dt;
            if( xDot_ * dt < -dx )
            {
                xDot_ = -dx / dt;
                if( debug ) printf( "x" );
            }
        }
    }
    else if( dx < 0 )
    {
        if( dx < -decelDx || xDot_ > 0 )
        {
            if( debug ) printf( "2a" );
            xDot_ -= xAccel * dt;
            if( xDot_ * dt < dx )
            {
                xDot_ = dx / dt;
                if( debug ) printf( "x" );
            }
        }
        else
        {
            if( debug ) printf( "2b" );
            xDot_ += xAccel * dt;
            if( xDot_ * dt > -dx )
            {
                xDot_ = -dx / dt;
                if( debug ) printf( "x" );
            }
        }
    }
    else if( dx == 0 )
    {
        if( debug ) printf( "3" );
        xDot_ = 0;
    }
    xDot_ = AuvMath::Limit( xDot_, -xdotmax, xdotmax );

    if( debug ) printf( " For dt=%g, x_=%f, xf=%f, dx=%f, decelDx=%f, xDot_=%f\n", dt, x_, xf, dx, decelDx, xDot_ );

    x_ += xDot_ * dt;

    if( periodic_ )
        x_ = AuvMath::ModPi( x_ );

    return x_ ;
}