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

#include "DeadReckonUsingDVLWaterTrack.h"
#include "DeadReckonUsingDVLWaterTrackIF.h"

#include "sensorModule/DVL_microIF.h"

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

DeadReckonUsingDVLWaterTrack::DeadReckonUsingDVLWaterTrack( const Module* module )
    : DeadReckoner( DeadReckonUsingDVLWaterTrackIF::NAME, module )
{
    // config readers
    verbosityLevelConfigReader_ = newConfigReader( DeadReckonUsingDVLWaterTrackIF::VERBOSITY_CFG );
    allowableFailuresConfigReader_ = newConfigReader( DeadReckonUsingDVLWaterTrackIF::ALLOWABLE_FAILURES_CFG );
    accuracyPremultiplierConfigReader_ = newConfigReader( DeadReckonUsingDVLWaterTrackIF::ACCURACY_PREMULTIPLIER_CFG );
    orientationStaleAfterConfigReader_ = newConfigReader( DeadReckonUsingDVLWaterTrackIF::ORIENTATION_STALE_AFTER_CFG );
    velocityStaleAfterConfigReader_ = newConfigReader( DeadReckonUsingDVLWaterTrackIF::VELOCITY_STALE_AFTER_CFG );
    // universal readers
    velocityReader_ = newBlobReaderFromUniversal( DVL_microIF::NAME, UniversalURI::PLATFORM_VELOCITY_WRT_SEA_WATER );
    // writers
    elapsedSinceOrientationReadWriter_ = newDataWriter( DeadReckonUsingDVLWaterTrackIF::ELAPSED_SINCE_ORIENTATION_READ );
    elapsedSinceVelocityReadWriter_ = newDataWriter( DeadReckonUsingDVLWaterTrackIF::ELAPSED_SINCE_VELOCITY_READ );
}

DeadReckonUsingDVLWaterTrack::~DeadReckonUsingDVLWaterTrack()
{}

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

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

    readConfigs();

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

void DeadReckonUsingDVLWaterTrack::readVehicleVelocity( void )
{
    speedAccuracy_ = 0.1; // TODO: ‘class DataReader’ has no member named ‘getAccuracy’
    // speedAccuracy_ = uReader_->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::NOT_SET_TIME; // DVL_Micro cannot be trusted to report its water velocities invalid. Do not use them as universal under any circumstances.
        speed_ = velocityRelativeToGroundInVehicleFrame_.getMagnitude();
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "DVL water track data is invalid.", 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_;
    }
}

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