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

#include "DeadReckonUsingCompactModelForecast.h"
#include "DeadReckonUsingCompactModelForecastIF.h"

// #include "estimationModule/HFRadarModelIF.h"

#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"
#include "data/Location.h" // XXX because we need to overload Deadreckoner::deadReckon

DeadReckonUsingCompactModelForecast::DeadReckonUsingCompactModelForecast( const Module* module )
    : DeadReckoner( DeadReckonUsingCompactModelForecastIF::NAME, module )
{
    // config readers
    verbosityLevelConfigReader_ = newConfigReader( DeadReckonUsingCompactModelForecastIF::VERBOSITY_CFG );
    allowableFailuresConfigReader_ = newConfigReader( DeadReckonUsingCompactModelForecastIF::ALLOWABLE_FAILURES_CFG );
    accuracyPremultiplierConfigReader_ = newConfigReader( DeadReckonUsingCompactModelForecastIF::ACCURACY_PREMULTIPLIER_CFG );
    orientationStaleAfterConfigReader_ = newConfigReader( DeadReckonUsingCompactModelForecastIF::ORIENTATION_STALE_AFTER_CFG );
    velocityStaleAfterConfigReader_ = newConfigReader( DeadReckonUsingCompactModelForecastIF::VELOCITY_STALE_AFTER_CFG );
    // universal readers
    speedWaterReader_ = newUniversalReader( UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER ); // TODO: consider using component-level URI explicitly
    uForecastReader_ = newUniversalReader( UniversalURI::SURFACE_NORTHWARD_SEA_WATER_VELOCITY ); // TODO: consider using component-level URI explicitly
    vForecastReader_ = newUniversalReader( UniversalURI::SURFACE_EASTWARD_SEA_WATER_VELOCITY ); // TODO: consider using component-level URI explicitly
    // Note that the convention for u & v here is switched from the CM Forecast component, so that it is consitent with navigation conventions in a North-East-Down reference frame.
    // writers
    elapsedSinceOrientationReadWriter_ = newDataWriter( DeadReckonUsingCompactModelForecastIF::ELAPSED_SINCE_ORIENTATION_READ );
    elapsedSinceVelocityReadWriter_ = newDataWriter( DeadReckonUsingCompactModelForecastIF::ELAPSED_SINCE_VELOCITY_READ );
}

DeadReckonUsingCompactModelForecast::~DeadReckonUsingCompactModelForecast()
{}

void DeadReckonUsingCompactModelForecast::initialize( void )
{
    logger_.syslog( "Initializing DeadReckonUsingCompactModelForecast component." );
    startTime_ = Timestamp::Now();

    vehicleOrientationReader_->requestData( true );
    speedWaterReader_->requestData( true );
    uForecastReader_->requestData( true );
    vForecastReader_->requestData( true );

    readConfigs();

    this->setAllowableFailures( allowableFailures_ );
    this->setFailureMissionCritical( false );
}

void DeadReckonUsingCompactModelForecast::readVehicleVelocity( void )
{
    bool speedWaterGood, uForecastGood, vForecastGood;
    float speedRelativeToWater, uForecast, vForecast;

    speedAccuracy_ = speedWaterReader_->getAccuracy( Units::METER_PER_SECOND );
    speedWaterGood = speedWaterReader_->read( Units::METER_PER_SECOND, speedRelativeToWater );

    speedAccuracy_ += uForecastReader_->getAccuracy( Units::METER_PER_SECOND );
    uForecastGood = uForecastReader_->read( Units::METER_PER_SECOND, uForecast );
    vForecastGood = vForecastReader_->read( Units::METER_PER_SECOND, vForecast );

    if( speedWaterGood && uForecastGood && vForecastGood )
    {
        velocityReadTime_ = Timestamp::Now();

        // For this DR method, assume the course of the vehicle is along its longitudinal axis.
        velocityRelativeToWaterInVehicleFrame_.setU( speedRelativeToWater );
        velocityRelativeToWaterInVehicleFrame_.setV( 0.0 );
        velocityRelativeToWaterInVehicleFrame_.setW( 0.0 );

        // TODO: The following velocities are not technically vehicle velocities -- clean this up. (Probably just rename the read method...)
        velocityOfWaterRelativeToGroundInNavigationFrame_.setU( uForecast );
        velocityOfWaterRelativeToGroundInNavigationFrame_.setV( vForecast );
        velocityOfWaterRelativeToGroundInNavigationFrame_.setW( 0.0 );
    }
    else if( speedWaterGood )
    {
        velocityReadTime_ = Timestamp::Now();

        if( verbosity_ > 0 ) logger_.syslog( "Invalid data for compact model forecast at this location, assuming zero current.", Syslog::DEBUG );

        // For this DR method, assume the course of the vehicle is along its longitudinal axis.
        velocityRelativeToWaterInVehicleFrame_.setU( speedRelativeToWater );
        velocityRelativeToWaterInVehicleFrame_.setV( 0.0 );
        velocityRelativeToWaterInVehicleFrame_.setW( 0.0 );

        // TODO: The following velocities are not technically vehicle velocities -- clean this up. (Probably just rename the read method...)
        velocityOfWaterRelativeToGroundInNavigationFrame_.setU( 0.0 );
        velocityOfWaterRelativeToGroundInNavigationFrame_.setV( 0.0 );
        velocityOfWaterRelativeToGroundInNavigationFrame_.setW( 0.0 );
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "Invalid data for speed through water and/or compact model forecast at this location.", Syslog::DEBUG );
        speedAccuracy_ = nan( "" );
    }
    if( accuracyPremultiplierConfigReader_->read( Units::NONE, accuracyPremultiplier_ ) ) // If you read it each cycle, you should be able to change it on the fly using ConfigSet.
    {
        speedAccuracy_ *= accuracyPremultiplier_;
    }
}

void DeadReckonUsingCompactModelForecast::deadReckon( void ) // override method DeadReckoner::deadReckon from parent class. *Most of this is identical.*
{
    elapsedTime_ = AuvMath::NanZero( dt_.asDouble() ); // dt_.asDouble() is nan for the first cycle

    /* XXX start of block that might be made more modular XXX */
    // calculate the velocity over the last computation cycle (in the navigation frame)
    velocityRelativeToWaterInNavigationFrame_ = 0; // zero this vector before using Point3D::addProduct on the next line
    velocityRelativeToWaterInNavigationFrame_.addProduct( rotationFromVehicleToNavigationFrame_, velocityRelativeToWaterInVehicleFrame_ );
    // add in the contribution from compact model forecast of surface currents
    velocityRelativeToGroundInNavigationFrame_ = velocityRelativeToWaterInNavigationFrame_ + velocityOfWaterRelativeToGroundInNavigationFrame_;
    /* XXX end of block that might be made more modular XXX */

    speed_ = velocityRelativeToGroundInNavigationFrame_.getMagnitude();

    // calculate the displacement since the last computation cycle (in the navigation frame)
    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_ ); // TODO: where do these Location:: methods get the start point?
    // 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_;
    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_.

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

// all other methods (including "run") inherited from DeadReckoner class