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

#include "DeadReckonWithRespectToWater.h"
#include "DeadReckonWithRespectToWaterIF.h"

#include "data/ConfigReader.h"
#include "data/Point3D.h" // TODO: replace with alternate LA tool
#include "data/UniversalDataReader.h"
#include "units/Units.h"

DeadReckonWithRespectToWater::DeadReckonWithRespectToWater( const Module* module )
    : DeadReckoner( DeadReckonWithRespectToWaterIF::NAME, module )
{
    // config readers
    verbosityLevelConfigReader_ = newConfigReader( DeadReckonWithRespectToWaterIF::VERBOSITY_CFG );
    allowableFailuresConfigReader_ = newConfigReader( DeadReckonWithRespectToWaterIF::ALLOWABLE_FAILURES_CFG );
    accuracyPremultiplierConfigReader_ = newConfigReader( DeadReckonWithRespectToWaterIF::ACCURACY_PREMULTIPLIER_CFG );
    orientationStaleAfterConfigReader_ = newConfigReader( DeadReckonWithRespectToWaterIF::ORIENTATION_STALE_AFTER_CFG );
    velocityStaleAfterConfigReader_ = newConfigReader( DeadReckonWithRespectToWaterIF::VELOCITY_STALE_AFTER_CFG );
    // universal readers
    speedReader_ = newUniversalReader( UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER );
    //writers
    elapsedSinceOrientationReadWriter_ = newDataWriter( DeadReckonWithRespectToWaterIF::ELAPSED_SINCE_ORIENTATION_READ );
    elapsedSinceVelocityReadWriter_ = newDataWriter( DeadReckonWithRespectToWaterIF::ELAPSED_SINCE_VELOCITY_READ );
    latitudeAccuracyWriter_ = newDataWriter( DeadReckonWithRespectToWaterIF::LATITUDE_ACCURACY );
}

DeadReckonWithRespectToWater::~DeadReckonWithRespectToWater()
{}

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

    vehicleOrientationReader_->requestData( true );
    speedReader_->requestData( true ); // why don't we request data from the speedReader in the old Navigation.cpp?

    readConfigs();

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

    // Just reading once at init since this variable is unlikely to change much on the fly.
    // If it's not set via config, we'll use the initialized value in Navigator.
    if( !( accuracyPremultiplierConfigReader_->read( Units::NONE, accuracyPremultiplier_ ) ) )
    {
        logger_.syslog( "Cannot read accuracy multiplier", Syslog::FAULT );
    }
}

void DeadReckonWithRespectToWater::readVehicleVelocity( void )
{
    float speedWater;

    if( speedReader_->read( Units::METER_PER_SECOND, speedWater ) )
    {
        velocityReadTime_ = Timestamp::Now();
        speedAccuracy_ = speedReader_->getAccuracy( Units::METER_PER_SECOND );
        velocityRelativeToGroundInVehicleFrame_.setU( speedWater ); // For this DR method, assume the course of the vehicle is along its longitudinal axis.
        velocityRelativeToGroundInVehicleFrame_.setV( 0.0 );
        velocityRelativeToGroundInVehicleFrame_.setW( 0.0 );
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "Data for platform speed with respect to sea water is invalid.", Syslog::ERROR );
        speedAccuracy_ = nan( "" );
    }

    // Increment the accuracy value to continue reflecting a decrease in accuracy.
    speedAccuracy_ *= accuracyPremultiplier_;

    /*    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_;
        }
    */

}

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