/** \file
 *
 *  Contains the SpeedCalculator class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "SpeedCalculator.h"
#include "SpeedCalculatorIF.h"

#include "controlModule/SpeedControlIF.h"
#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "utils/Timestamp.h"

SpeedCalculator::SpeedCalculator( const Module* module )
    : SyncDerivationComponent( SpeedCalculatorIF::NAME, module ),
      propOmega_( 0.0 ),
      propPitch_( 0.0 ),
      speed_( 0.0 ),
      speedAccuracy_( 0.5 ),
      distance_( 0.0 ),
      distanceAccuracy_( 0.0 )
{

    // universal readers
    propOmegaReader_ = newUniversalReader( UniversalURI::PLATFORM_PROPELLER_ROTATION_RATE );

    // universal writers
    speedWriter_ = newUniversalWriter( UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER, Units::METER_PER_SECOND, nanf( "" ) ); // initialize writer with bad accuracy so that it does not overwrite universals with better accuracy
    xVelocityWriter_ = newUniversalWriter( UniversalURI::PLATFORM_X_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, nanf( "" ) );
    distanceWriter_ = newUniversalWriter( UniversalURI::PLATFORM_DISTANCE_WRT_SEA_WATER, Units::METER_PER_SECOND, nanf( "" ) ); // initialize writer with bad accuracy so that it does not overwrite universals with better accuracy

    // Configuration settings
    speedAccuracyCfgReader_ = newConfigReader( SpeedCalculatorIF::SPEED_ACCURACY_CFG );
    propPitchCfgReader_ = newConfigReader( SpeedControlIF::PROP_PITCH_CFG );

}

SpeedCalculator::~SpeedCalculator()
{}

void SpeedCalculator::initialize( void )
{
    logger_.syslog( "Initializing SpeedCalculator." );
}

void SpeedCalculator::run( void )
{
    // Read in common parameters
    double timeElapsed( dt_.asDouble() );
    if( timeElapsed != timeElapsed )
    {
        return;
    }

    if( propOmegaReader_->isActive() )
    {
        propOmegaReader_->read( Units::RADIAN_PER_SECOND, propOmega_ );
    }
    else
    {
        return;
    }

    propPitchCfgReader_->read( Units::METER_PER_RADIAN, propPitch_ ); // read every cycle so it can be changed on the fly
    speedAccuracyCfgReader_->read( Units::METER_PER_SECOND, speedAccuracy_ ); // read every cycle so it can be changed on the fly

    speed_ = propOmega_ * propPitch_;

    if( isnan( speed_ ) )
    {
        speedWriter_->setInvalid( true );
        xVelocityWriter_->setInvalid( true );
    }
    else
    {
        speedWriter_->setInvalid( false );
        speedWriter_->writeWithAccuracy( Units::METER_PER_SECOND, speed_, speedAccuracy_ );
        xVelocityWriter_->setInvalid( false );
        xVelocityWriter_->writeWithAccuracy( Units::METER_PER_SECOND, speed_, speedAccuracy_ );
    }

    distance_ += speed_ * timeElapsed;
    distanceAccuracy_ += speedAccuracy_ * timeElapsed;

    if( isnan( distance_ ) || isnan( distanceAccuracy_ ) )
    {
        distance_ = 0;
        distanceAccuracy_ = 0;
        distanceWriter_->setInvalid( true );
    }
    else
    {
        distanceWriter_->setInvalid( false );
        distanceWriter_->writeWithAccuracy( Units::METER_PER_SECOND, distance_, distanceAccuracy_ );
    }

}