/** \file
 *
 *  Contains the DeadReckoner class implementation.
 *
 *  Copyright (c) 2013 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "DeadReckoner.h"

#include "data/Location.h"
#include "data/Matrix3x3.h" // TODO: replace with alternate LA tool
#include "data/Point3D.h" // TODO: replace with alternate LA tool

DeadReckoner::DeadReckoner( const Str& name, const Module* module )
    : Navigator( name, module ), // This is intended to be a superclass for all dead reckoners...
      elapsedTime_( 0.0 ),
      distance_( nan( "" ) ),
      northing_( nan( "" ) ),
      easting_( nan( "" ) ),
      horizontalDisplacement_( nan( "" ) )
{}

DeadReckoner::~DeadReckoner()
{}

void DeadReckoner::run( void )
{
    // set the data timestamp
    navTimeStamp_ = Timestamp::Now();
    // read inputs
    readVehicleOrientation(); // inherited from Navigator superclass
    readVehicleVelocity(); // needs to be overridden for each DeadReckon* subclass
    // perform calculation
    deadReckon(); // specified for all DeadReckoners in this class
    // check for a fix
    readSetNavFix(); // inherited from Navigator superclass
    // check for a fix
    readExternalFix(); // inherited from Navigator superclass
    // write outputs
    writeVehiclePosition(); // inherited from Navigator superclass
}

void DeadReckoner::deadReckon( void )
{
    elapsedTime_ = AuvMath::NanZero( dt_.asDouble() ); // dt_.asDouble() is nan for the first cycle
    distance_ = speed_ * elapsedTime_; // TODO: Am I actually using this anywhere? should it get written to the slate?

    // calculate the displacement since the last computation cycle (in the navigation frame)
    velocityRelativeToGroundInNavigationFrame_ = 0; // zero this vector before using Point3D::addProduct on the next line
    velocityRelativeToGroundInNavigationFrame_.addProduct( rotationFromVehicleToNavigationFrame_, velocityRelativeToGroundInVehicleFrame_ );
    displacementRelativeToGroundInNavigationFrame_ = velocityRelativeToGroundInNavigationFrame_ * elapsedTime_;

    // calculate the new depth
    depth_ = lastDepth_ + displacementRelativeToGroundInNavigationFrame_.getZ();

    // calculate the course and horizontal displacement
    northing_ = displacementRelativeToGroundInNavigationFrame_.getX();
    easting_ = displacementRelativeToGroundInNavigationFrame_.getY();
    course_ = atan2( easting_, northing_ ); // TODO: Do we actually use course anywhere? should we write it to the slate?
    horizontalDisplacement_ = sqrt( pow( northing_, 2 ) + pow( easting_, 2 ) );
    horizontalPathLengthSinceLastFix_ += horizontalDisplacement_;

    // calculate the new latitude and longitude
    Location::AtBearing( course_, horizontalDisplacement_, latitude_, longitude_ );
    // Location::Delta( latitude_, longitude_, northing_, easting_ ); // Alternate, but this one would just result in more calls to atan2 and sqrt.

    // set output accuracies
    latitudeAccuracy_ += speedAccuracy_ * elapsedTime_ * Location::EARTH_RADIUS_INVERTED;
    longitudeAccuracy_ += speedAccuracy_ * elapsedTime_ * Location::EARTH_RADIUS_INVERTED;

    depthAccuracy_ += speedAccuracy_ * elapsedTime_;
    courseAccuracy_ =  !isnan( speedAccuracy_ ) && !isnan( speed_ ) ? atan2( speedAccuracy_, speed_ ) : nanf( "" );
    horizontalPathLengthSinceLastFixAccuracy_ += speedAccuracy_ * elapsedTime_;
    // TODO: Revisit this accuracy math -- the rough use of speedAccuracy_ here may not be quite right, and there is no accounting for orientationAccuracy_.

    // Now check to see if speed is actually back from being nan so that we can reset the depth and horizontal path accuracy and pick up dead reckoning again.
    // If the two things we care about are nan but the stuff we use to calculate them aren't anymore, then let's reset just like
    // we do when there's a GPS fix.
    if( !isnan( speedAccuracy_ ) && !isnan( elapsedTime_ ) && isnan( depthAccuracy_ ) && isnan( horizontalPathLengthSinceLastFixAccuracy_ ) )
    {
        depthAccuracy_ = 1.0e6; // reset this to something large -- want to rely on Keller & CTD in all but the most dire of circumstances...
        horizontalPathLengthSinceLastFixAccuracy_ = 1.0; // reset this to something reasonable
    }

    // preserve computed position for next computation cycle
    lastLatitude_ = latitude_;
    lastLongitude_ = longitude_;
    lastDepth_ = depth_;
}