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

#include "YawRateCalculator.h"
#include "YawRateCalculatorIF.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

YawRateCalculator::YawRateCalculator( const Module* module )
    : SyncDerivationComponent( YawRateCalculatorIF::NAME, module ),
      lastOrientation_( nanf( "" ) )
{

    // accessors
    orientationReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION );
    yawRateWriter_ = newUniversalWriter( UniversalURI::PLATFORM_YAW_RATE );
}

YawRateCalculator::~YawRateCalculator()
{}

void YawRateCalculator::initialize( void )
{
    logger_.syslog( "Initializing YawRateCalculator." );
    orientationReader_->requestData( true );
}

void YawRateCalculator::run( void )
{

    float orientation( nan( "" ) );
    orientationReader_->read( Units::RADIAN, orientation );
    if( !isnan( orientation ) && !isnan( lastOrientation_ ) )
    {
        float elapsed = dt_.asDouble();
        if( elapsed == elapsed )
        {
            yawRateWriter_->write( Units::RADIAN_PER_SECOND, AuvMath::ModPi( orientation - lastOrientation_ ) / elapsed );
            float orientationAccuracy = orientationReader_->getAccuracy( Units::RADIAN );
            if( orientationAccuracy / elapsed != yawRateWriter_->getAccuracy( Units::RADIAN_PER_SECOND ) )
            {
                yawRateWriter_->setAccuracy( Units::RADIAN_PER_SECOND,
                                             orientationAccuracy / elapsed * BACK_DIFFERENCE_ERROR_FACTOR );
            }
        }
    }
    lastOrientation_ = orientation;
}