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

#include "Depth_Keller.h"
#include "Depth_KellerIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "sensorModule/OnboardIF.h"

#define KELLER_ACCURACY_DEPTH (0.02) // This has not been calibrated yet
#define KELLER_ACCURACY_PRESSURE KELLER_ACCURACY_DEPTH

Depth_Keller::Depth_Keller( const Module* module )
    : SyncSensorComponent( Depth_KellerIF::NAME, module ),
      depthPressBoundingErrCnt_( 0 ),
      boundingErrMaxCnt_( 5 ),
      pressureOffset_( 0 ),
      scale_( 27.72 ), // will be overwritten by config value
      maxPressBound_( 499 ),
      minPressBound_( -9 ),
      minDepthBound_( -0.5 ),
      adPressure_( Depth_KellerIF::AD, Depth_KellerIF::AD_VREF, Depth_KellerIF::AD_RES, Depth_KellerIF::AD_TIMEOUT, !simulateHardware(), logger_ ),
      loadControl_( Depth_KellerIF::LOAD_CONTROL, !simulateHardware(), logger_, this )
{
    setRepeater( true );

    // data readers
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE ); // used in converting pressure to depth
    // data writers
    depthWriter_ = newUniversalWriter( UniversalURI::DEPTH, Units::METER, KELLER_ACCURACY_DEPTH );
    pressureWriter_ = newUniversalWriter( UniversalURI::SEA_WATER_PRESSURE, Units::DECIBAR, KELLER_ACCURACY_PRESSURE );
    // config readers
    offsetCfgReader_ = newConfigReader( Depth_KellerIF::OFFSET_CFG );
    scaleCfgReader_ = newConfigReader( Depth_KellerIF::SCALE_CFG );
    maxPressBoundCfgReader_ = newConfigReader( Depth_KellerIF::MAX_PRESS_BOUND_CFG );
    minPressBoundCfgReader_ = newConfigReader( Depth_KellerIF::MIN_PRESS_BOUND_CFG );

}

Depth_Keller::~Depth_Keller()
{}

void Depth_Keller::readConfig()
{
    offsetCfgReader_->read( Units::DECIBAR, pressureOffset_ );
    scaleCfgReader_->read( Units::MICROBAR, scale_ );
    maxPressBoundCfgReader_->read( Units::DECIBAR, maxPressBound_ );
    minPressBoundCfgReader_->read( Units::DECIBAR, minPressBound_ );
}

void Depth_Keller::initialize()
{
    this->setAllowableFailures( 3 );
    readConfig();
    depthPressBoundingErrCnt_ = 0;
    if( !simulateHardware() )
    {
        adPressure_.startRead();
        loadControl_.deisolateLoad();
    }
}

void Depth_Keller::run()
{
    readConfig();
    if( isRepeating() )
    {
        adPressure_.startRead();
        return;
    }

    float latitude;
    bool haveDepth = false;
    bool havePressure = false;
    bool haveLocation = latitudeReader_->isActive()
                        && latitudeReader_->read( Units::RADIAN, latitude );

    float depth;
    float pressure( nanf( "" ) );

    // Just to double check that latitude isn't nan
    if( haveLocation && isnan( latitude ) )
    {
        haveLocation = false;
        logger_.syslog( "Latitude is nan. Will use worksite latitude.", Syslog::ERROR );
    }

    if( !simulateHardware() )
    {
        // Read ADC and convert to pressure
        pressure = adPressure_.read( scale_ );
        pressure += pressureOffset_;

        if( isnan( pressure ) )
        {
            setFailure( FailureMode::DATA );
            return;
        }

        if( ( pressure < maxPressBound_ ) && ( pressure > minPressBound_ ) )
        {
            havePressure = true;
            if( haveLocation )
            {
                depth = AuvMath::OceanDepth( pressure * 10000.0, latitude );
            }
            else // No location so use defaults
            {
                float initLat;
                Slate::ReadOnce( "Config/workSite", "initLat", Units::RADIAN, initLat, logger_ );
                depth = AuvMath::OceanDepth( pressure * 10000.0, initLat );
            }
            if( depth < minDepthBound_ )
            {
                haveDepth = false;
            }
            else
            {
                haveDepth = true;

                depthPressBoundingErrCnt_ = 0;
                pressureWriter_->setInvalid( false );
                depthWriter_->setInvalid( false );
            }
        }
        if( !havePressure || !haveDepth )
        {
            pressureWriter_->setInvalid( true );
            depthWriter_->setInvalid( true );
            if( depthPressBoundingErrCnt_ == 0 )
            {
                logger_.syslog( Str( "Pressure or depth reading out of range: " + ( Str )pressure + " decibar, " + ( Str )depth + " m" ), Syslog::ERROR );
            }
            else if( depthPressBoundingErrCnt_ >= boundingErrMaxCnt_ )
            {
                logger_.syslog( Str( "Pressure or depth reading out of range for max " + ( Str )boundingErrMaxCnt_ + " samples" ), Syslog::FAULT );
                setFailure( FailureMode::DATA );
            }
            depthPressBoundingErrCnt_++;
        }

    }

    // Always include simulated values, even if hardware exists.
    // If we are in the water, there will be no simulated values available.
    if( SimSlate::Read( SimSlate::DEPTH_METER, depth ) )
    {
        haveDepth = true;
        if( haveLocation )
        {
            pressure = AuvMath::OceanPressure( depth, latitude );
            pressure /= 1e4;
            havePressure = true;
        }
    }

    if( haveDepth && havePressure )
    {
        depthWriter_->write( Units::METER, depth );
        pressureWriter_->write( Units::DECIBAR, pressure );
        this->resetFailCount();
    }
}

void Depth_Keller::uninitialize()
{}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI Depth_Keller::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_POWER:
        return Depth_KellerIF::POWER;
    case CONFIG_SIMULATE_HARDWARE:
        return Depth_KellerIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
}