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

#include "PitchRateCalculator.h"
#include "PitchRateCalculatorIF.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"

#define BACK_DIFFERENCE_ERROR_FACTOR 50.0

PitchRateCalculator::PitchRateCalculator( const Module* module )
    : SyncDerivationComponent( PitchRateCalculatorIF::NAME, module ),
      lastPitch_( nanf( "" ) )
{
    pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE );
    pitchRateWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PITCH_RATE );
}

PitchRateCalculator::~PitchRateCalculator()
{}

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

void PitchRateCalculator::run( void )
{
    float pitch( nan( "" ) );
    pitchReader_->read( Units::RADIAN, pitch );
    if( !isnan( pitch ) && !isnan( lastPitch_ ) )
    {
        float elapsed = dt_.asDouble();
        if( elapsed == elapsed )
        {
            pitchRateWriter_->write( Units::RADIAN_PER_SECOND, AuvMath::ModPi( pitch - lastPitch_ ) / elapsed );
            float pitchAccuracy = pitchReader_->getAccuracy( Units::RADIAN );
            if( pitchAccuracy / elapsed != pitchRateWriter_->getAccuracy( Units::RADIAN_PER_SECOND ) )
            {
                pitchRateWriter_->setAccuracy( Units::RADIAN_PER_SECOND,
                                               pitchAccuracy / elapsed * BACK_DIFFERENCE_ERROR_FACTOR );
            }
        }
    }
    lastPitch_ = pitch;
}