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

#include "DeadReckonUsingMultipleVelocitySources.h"
#include "DeadReckonUsingMultipleVelocitySourcesIF.h"

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

#include "derivationModule/SpeedCalculatorIF.h" // To read component-level writer directly, in case of an asynchronous timing event where speed from DVL is marked invalid, but Universal has not yet been reassigned to the SpeedCalculator.

DeadReckonUsingMultipleVelocitySources::DeadReckonUsingMultipleVelocitySources( const Module* module )
    : DeadReckoner( DeadReckonUsingMultipleVelocitySourcesIF::NAME, module )
{
    // config readers
    verbosityLevelConfigReader_ = newConfigReader( DeadReckonUsingMultipleVelocitySourcesIF::VERBOSITY_CFG );
    allowableFailuresConfigReader_ = newConfigReader( DeadReckonUsingMultipleVelocitySourcesIF::ALLOWABLE_FAILURES_CFG );
    accuracyPremultiplierConfigReader_ = newConfigReader( DeadReckonUsingMultipleVelocitySourcesIF::ACCURACY_PREMULTIPLIER_CFG );
    orientationStaleAfterConfigReader_ = newConfigReader( DeadReckonUsingMultipleVelocitySourcesIF::ORIENTATION_STALE_AFTER_CFG );
    velocityStaleAfterConfigReader_ = newConfigReader( DeadReckonUsingMultipleVelocitySourcesIF::VELOCITY_STALE_AFTER_CFG );
    // universal readers
    velocityBottomReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_VELOCITY_WRT_GROUND );
    velocityWaterReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_VELOCITY_WRT_SEA_WATER );
    speedCalculatorReader_ = newDataReaderFromUniversal( SpeedCalculatorIF::NAME, UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER );
    // writers
    elapsedSinceOrientationReadWriter_ = newDataWriter( DeadReckonUsingMultipleVelocitySourcesIF::ELAPSED_SINCE_ORIENTATION_READ );
    elapsedSinceVelocityReadWriter_ = newDataWriter( DeadReckonUsingMultipleVelocitySourcesIF::ELAPSED_SINCE_VELOCITY_READ );
    latitudeAccuracyWriter_ = newDataWriter( DeadReckonUsingMultipleVelocitySourcesIF::LATITUDE_ACCURACY );
    velocitySourceWriter_ = newDataWriter( DeadReckonUsingMultipleVelocitySourcesIF::VELOCITY_SOURCE );
}

DeadReckonUsingMultipleVelocitySources::~DeadReckonUsingMultipleVelocitySources()
{}

void DeadReckonUsingMultipleVelocitySources::initialize( void )
{
    startTime_ = Timestamp::Now();

    vehicleOrientationReader_->requestData( true );
    velocityBottomReader_->requestData( true );
    velocityWaterReader_->requestData( true );
    speedCalculatorReader_->requestData( true );

    velocitySource_ = 0;

    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_ ) ) )
    {
        speedAccuracy_ *= 0.9; // Adjust accuracies by a tiny amount to make this the preferred method for DR and to avoid conflicts when two navigators repeatedly write position with the same accuracy.
        logger_.syslog( "Cannot read accuracy multiplier", Syslog::FAULT );
        if( verbosity_ > 1 ) logger_.syslog( "using accuracy premultiplier 0.9 to encourage the universal to use this DR method and avoid conflicts right after a position fix", Syslog::DEBUG );
    }
}

void DeadReckonUsingMultipleVelocitySources::readVehicleVelocity( void )
{
    // NOTE: see line with speedAccuracy_ at the bottom of this method.
    // TODO: More precise & correct definition of speed accuracy. (Also a side note: are we talking about accuracy or precision here?)

    // (try to) read from each of the velocity sources in turn
    if( velocityBottomReader_->read1DClass( Units::METER_PER_SECOND, velocityRelativeToGroundInVehicleFrame_ ) ) // directionToContactReader_->read1DClass( Units::NONE, directionInFSK_ );
    {
        speedAccuracy_ = velocityBottomReader_->getAccuracy( Units::METER_PER_SECOND ); // read accuracy right after value to minimize chance of asynchronous change
        speed_ = velocityRelativeToGroundInVehicleFrame_.getMagnitude();
        velocitySource_ = 0;
        velocityReadTime_ = Timestamp::Now();
    }
    else if( ( velocitySource_ == 0 ) && ( velocityReadTime_.elapsed() < velocityStaleAfter_ ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Bottom track data is " + Str( velocityReadTime_.elapsed().asDouble(), 1 ) + " s old, using for " + Str( velocityStaleAfter_.asDouble(), 1 ) + " s.", Syslog::DEBUG );
        // leave velocityRelativeToGroundInVehicleFrame_, speedAccuracy_, speed_ untouched for another cycle.
    }
    /*    else if( velocityWaterReader_->read1DClass( Units::METER_PER_SECOND, velocityRelativeToGroundInVehicleFrame_ ) )
        {
            speedAccuracy_ = velocityWaterReader_->getAccuracy( Units::METER_PER_SECOND ); // read accuracy right after value to minimize chance of asynchronous change
            speed_ = velocityRelativeToGroundInVehicleFrame_.getMagnitude();
            velocitySource_ = 1;
            velocityReadTime_ = Timestamp::Now();
        }
        else if( ( velocitySource_ == 1 ) && ( velocityReadTime_.elapsed() < velocityStaleAfter_ ) )
        {
            if( verbosity_ > 0 ) logger_.syslog( "Water reference layer data is " + Str( velocityReadTime_.elapsed().asDouble(), 1 ) + " s old, using for " + Str( velocityStaleAfter_.asDouble(), 1 ) + " s.", Syslog::DEBUG );
            // leave velocityRelativeToGroundInVehicleFrame_, speedAccuracy_, speed_ untouched for another cycle.
        }
    */
    else if( speedCalculatorReader_->read( Units::METER_PER_SECOND, speed_ ) )
    {
        speedAccuracy_ = 0.25; // TODO: Enable reading accuracy from a component-level data source.
        // Use speed relative to seawater as velocity relative to ground, even though it is not quite right.
        // This is explicitly from the SpeedCalculator component.
        velocityRelativeToGroundInVehicleFrame_.setU( speed_ ); // For this DR method, assume the course of the vehicle is along its longitudinal axis.
        velocityRelativeToGroundInVehicleFrame_.setV( 0.0 );
        velocityRelativeToGroundInVehicleFrame_.setW( 0.0 );
        velocitySource_ = 3;
        velocityReadTime_ = Timestamp::Now();
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "All data for platform velocity is invalid.", Syslog::DEBUG );
        speedAccuracy_ = nan( "" );
        velocitySourceWriter_->write( Units::COUNT, 31, navTimeStamp_ ); // TODO: Consider using -1 instead if that is valid. Alternatively, bump everything up one or two...
    }
    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_;
        if( verbosity_ > 1 ) logger_.syslog( "using accuracyPremultiplier from config", Syslog::DEBUG );
    }
    // Increment the accuracy value to continue reflecting a decrease in accuracy.
    speedAccuracy_ *= accuracyPremultiplier_;

    // And finally, write the source
    velocitySourceWriter_->write( Units::COUNT, velocitySource_, navTimeStamp_ );
}
// all other methods (including "run") inherited from DeadReckoner class