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

#include "DeadReckonWithRespectToSeafloor.h"
#include "DeadReckonWithRespectToSeafloorIF.h"

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

DeadReckonWithRespectToSeafloor::DeadReckonWithRespectToSeafloor( const Module* module )
    : DeadReckoner( DeadReckonWithRespectToSeafloorIF::NAME, module )
{
    // config readers
    verbosityLevelConfigReader_ = newConfigReader( DeadReckonWithRespectToSeafloorIF::VERBOSITY_CFG );
    allowableFailuresConfigReader_ = newConfigReader( DeadReckonWithRespectToSeafloorIF::ALLOWABLE_FAILURES_CFG );
    accuracyPremultiplierConfigReader_ = newConfigReader( DeadReckonWithRespectToSeafloorIF::ACCURACY_PREMULTIPLIER_CFG );
    orientationStaleAfterConfigReader_ = newConfigReader( DeadReckonWithRespectToSeafloorIF::ORIENTATION_STALE_AFTER_CFG );
    velocityStaleAfterConfigReader_ = newConfigReader( DeadReckonWithRespectToSeafloorIF::VELOCITY_STALE_AFTER_CFG );
    // universal readers
    velocityReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_VELOCITY_WRT_GROUND );
    // writers
    elapsedSinceOrientationReadWriter_ = newDataWriter( DeadReckonWithRespectToSeafloorIF::ELAPSED_SINCE_ORIENTATION_READ );
    elapsedSinceVelocityReadWriter_ = newDataWriter( DeadReckonWithRespectToSeafloorIF::ELAPSED_SINCE_VELOCITY_READ );
    latitudeAccuracyWriter_ = newDataWriter( DeadReckonWithRespectToSeafloorIF::LATITUDE_ACCURACY );
}

DeadReckonWithRespectToSeafloor::~DeadReckonWithRespectToSeafloor()
{}


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

    vehicleOrientationReader_->requestData( true );
    velocityReader_->requestData( true );

    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 DeadReckonWithRespectToSeafloor::readVehicleVelocity( void )
{
    speedAccuracy_ = velocityReader_->getAccuracy( Units::METER_PER_SECOND ); // TODO: More precise & correct definition of speed accuracy. (Also a side note: are we talking about accuracy or precision here?)
    if( velocityReader_->read1DClass( Units::METER_PER_SECOND, velocityRelativeToGroundInVehicleFrame_ ) )
    {
        velocityReadTime_ = Timestamp::Now();
        speed_ = velocityRelativeToGroundInVehicleFrame_.getMagnitude();
    }
    else
    {
        if( verbosity_ > 1 ) logger_.syslog( "Data for platform velocity with respect to ground is invalid.", Syslog::ERROR );
        speedAccuracy_ = nan( "" );
    }

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

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