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

#include "DepthRateCalculator.h"
#include "DepthRateCalculatorIF.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"

DepthRateCalculator::DepthRateCalculator( const Module* module )
    : SyncDerivationComponent( DepthRateCalculatorIF::NAME, module ),
      lastDepth_( nanf( "" ) ),
      missedSecs_( 0.0 )
{
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    depthRateWriter_ = newUniversalWriter( UniversalURI::DEPTH_RATE );
}

DepthRateCalculator::~DepthRateCalculator()
{}

void DepthRateCalculator::initialize( void )
{
    logger_.syslog( "Initializing DepthRateCalculator." );
    depthReader_->requestData( DataAccessor::MIN_POWER );
}

void DepthRateCalculator::uninitialize( void )
{
    logger_.syslog( "Uninitializing DepthRateCalculator." );
    depthReader_->requestData( DataAccessor::NO_STRATEGY );
}

void DepthRateCalculator::run( void )
{
    if( depthReader_->isActive() )
    {
        float depth( nan( "" ) );
        depthReader_->read( Units::METER, depth );
        if( !isnan( depth ) && !isnan( lastDepth_ ) )
        {
            float elapsed = dt_.asDouble();
            if( !isnan( elapsed ) )
            {
                elapsed = missedSecs_ = elapsed + missedSecs_;
                float depthRate = ( depth - lastDepth_ ) / elapsed;
                if( depthRate != 0.0 )
                {
                    missedSecs_ = 0.0;
                    depthRateWriter_->write( Units::METER_PER_SECOND, depthRate );
                    float depthAccuracy = depthReader_->getAccuracy( Units::METER );
                    if( depthAccuracy / elapsed != depthRateWriter_->getAccuracy( Units::METER_PER_SECOND ) )
                    {
                        depthRateWriter_->setAccuracy( Units::METER_PER_SECOND, depthAccuracy / elapsed );
                    }
                }
            }
        }
        lastDepth_ = depth;
    }
    else
    {
        logger_.syslog( "Depth measurement is not active", Syslog::ERROR );
    }
    return;
}