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

#include "Pitch.h"
#include "PitchIF.h"

#include "controlModule/VerticalControlIF.h"

#include "data/Slate.h"
#include "data/UniversalURI.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

Pitch::Pitch( const Str& prefix, const Module* module )
    : Behavior( prefix + PitchIF::NAME, module, true, true ),
      /// Desired depth of the vehicle
      depthSetting_( nan( "" ) ),
      /// Desired depth rate of the vehicle
      depthRateSetting_( nan( "" ) ),
      /// Desired elevator angle of the vehicle
      elevatorAngleSetting_( nan( "" ) ),
      /// Desired massPosition of the vehicle
      massPositionSetting_( nan( "" ) ),
      /// Desired pitch of the vehicle
      pitchSetting_( nan( "" ) ),
      /// Desired pitch rate of the vehicle
      pitchRateSetting_( nan( "" ) ),
      /// Initial depth of the vehicle
      initialDepth_( nan( "" ) ),
      /// Current depth of the vehicle
      depth_( nan( "" ) ),
      /// Have we learned all our initial settings?
      initialized_( false )
{
    logger_.syslog( "Construct." );

    // Slate input setting variables
    depthSettingReader_ = newSettingReader( PitchIF::DEPTH_SETTING );
    depthRateSettingReader_ = newSettingReader( PitchIF::DEPTH_RATE_SETTING );
    elevatorAngleSettingReader_ = newSettingReader( PitchIF::ELEVATOR_ANGLE_SETTING );
    massPositionSettingReader_ = newSettingReader( PitchIF::MASS_POSITION_SETTING );
    pitchSettingReader_ = newSettingReader( PitchIF::PITCH_SETTING );
    pitchRateSettingReader_ = newSettingReader( PitchIF::PITCH_RATE_SETTING );

    // Slate input measurements
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // Slate output variables
    depthCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_CMD );
    depthRateCmdWriter_ = newDataWriter( VerticalControlIF::DEPTH_RATE_CMD );
    elevatorAngleCmdWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_CMD );
    massPositionCmdWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_CMD );
    pitchCmdWriter_ = newDataWriter( VerticalControlIF::PITCH_CMD );
    pitchRateCmdWriter_ = newDataWriter( VerticalControlIF::PITCH_RATE_CMD );
    verticalModeWriter_ = newDataWriter( VerticalControlIF::VERTICAL_MODE );
}

Pitch::~Pitch()
{}

/// Initialize function
void Pitch::initialize()
{
    logger_.syslog( "Initialize." );

    depthSetting_ = nan( "" );
    depthRateSetting_ = nan( "" );
    elevatorAngleSetting_ = nan( "" );
    massPositionSetting_ = nan( "" );
    pitchSetting_ = nan( "" );
    pitchRateSetting_ = nan( "" );
    initialDepth_ = nan( "" );

    readSettings();

    depthReader_->requestData( true );

}

// Reads in behavior settings
void Pitch::readSettings()
{
    // Let's read in all the relevant dataReaders
    depthSettingReader_->read( Units::METER, depthSetting_ );
    depthRateSettingReader_->read( Units::METER_PER_SECOND, depthRateSetting_ );
    elevatorAngleSettingReader_->read( Units::RADIAN, elevatorAngleSetting_ );
    massPositionSettingReader_->read( Units::METER, massPositionSetting_ );
    pitchSettingReader_->read( Units::RADIAN, pitchSetting_ );
    pitchRateSettingReader_->read( Units::RADIAN_PER_SECOND, pitchRateSetting_ );

    if( !initialized_ )
    {
        initialized_ = depthReader_->read( Units::METER, initialDepth_ )
                       && !isnan( initialDepth_ );
    }

}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool Pitch::readParams()
{
    readSettings();

    if( !initialized_ )
    {
        return false;
    }
    if( !isnan( depthSetting_ ) )
    {
        if( !depthReader_->read( Units::METER, depth_ ) )
        {
            return false;
        }
    }

    return true;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool Pitch::calcSatisfied()
{
    if( !isnan( depthSetting_ ) && !isnan( depth_ ) )
    {
        if( initialDepth_ <= depthSetting_ && depth_ >= depthSetting_ )
        {
            return  true;
        }
        else if( initialDepth_ > depthSetting_ && depth_ <= depthSetting_ )
        {
            return true;
        }
        if( !isnan( pitchSetting_ ) )
        {
            if( pitchSetting_ < 0 && depth_ >= depthSetting_ )
            {
                return  true;
            }
            else if( pitchSetting_ > 0 && depth_ <= depthSetting_ )
            {
                return true;
            }
        }
    }

    return false;
}

/// Just do the run
void Pitch::run()
{
    runIfUnsatisfied();
}

/// Just do the satisfied: return true if envelope "satisfied"
bool Pitch::isSatisfied()
{
    bool retVal( false );
    if( readParams() )
    {
        retVal = calcSatisfied();
    }
    return retVal;
}

/// Do the run, and return true if envelope "satisfied"
bool Pitch::runIfUnsatisfied()
{
    bool retVal( false );
    if( readParams() )
    {
        retVal = calcSatisfied();
        if( !retVal )
        {
            if( massPositionSettingReader_->isActive() )
            {
                /// If we've specified a massPostion, use it!
                massPositionCmdWriter_->write( Units::METER, massPositionSetting_ );
            }
            if( elevatorAngleSettingReader_->isActive() )
            {
                elevatorAngleCmdWriter_->write( Units::RADIAN, elevatorAngleSetting_ );
            }
            /// Control using only the specified massPosition and/or elevatorAngle
            if( isnan( depthSetting_ ) && isnan( pitchSetting_ ) && isnan( pitchRateSetting_ )
                    && ( !isnan( massPositionSetting_ ) && !isnan( elevatorAngleSetting_ ) ) )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::MASS_AND_ELEVATOR );
            }
            /// Maintain a set depth
            else if( !isnan( depthSetting_ ) && isnan( pitchSetting_ ) && isnan( pitchRateSetting_ ) )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH );
                depthCmdWriter_->write( Units::METER, depthSetting_ );
                if( !isnan( depthRateSetting_ ) )
                {
                    depthRateCmdWriter_->write( Units::METER_PER_SECOND, depthRateSetting_ );
                }
            }
            /// Maintain a set depth rate
            else if( !isnan( depthRateSetting_ ) )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH_RATE );
                depthRateCmdWriter_->write( Units::METER_PER_SECOND, depthRateSetting_ );
            }
            /// Maintain a set pitch
            else if( !isnan( pitchSetting_ ) )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::PITCH );
                pitchCmdWriter_->write( Units::RADIAN, pitchSetting_ );
                if( !isnan( pitchRateSetting_ ) )
                {
                    pitchRateCmdWriter_->write( Units::RADIAN_PER_SECOND, pitchRateSetting_ );
                }
            }
            /// Maintain a set pitch rate
            else if( !isnan( pitchRateSetting_ ) )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::PITCH_RATE );
                pitchRateCmdWriter_->write( Units::RADIAN_PER_SECOND, pitchRateSetting_ );
            }
            /// Maintain a zero pitch if nothing set
            else if( !massPositionSettingReader_->isActive() && !elevatorAngleSettingReader_->isActive() )
            {
                verticalModeWriter_->write( Units::ENUM, VerticalControlIF::PITCH_ZERO );
            }
        }
        else
        {
            /// We can only be satisfied in depth mode
            /// however we still need to maintain depth!
            if( !isnan( massPositionSetting_ ) )
            {
                /// If we've specified a massPostion, use it!
                massPositionCmdWriter_->write( Units::METER, massPositionSetting_ );
            }
            if( !isnan( elevatorAngleSetting_ ) )
            {
                elevatorAngleCmdWriter_->write( Units::METER, elevatorAngleSetting_ );
            }
            /// Depth is satisfiable -- if so, runIfUnsatisifed could quit --
            /// so we'll maintain depth here.
            verticalModeWriter_->write( Units::ENUM, VerticalControlIF::DEPTH );
            depthCmdWriter_->write( Units::METER, depthSetting_ );
        }
    }
    return retVal;
}

/// Uninit function
void Pitch::uninitialize()
{
    depthReader_->requestData( false );
    initialized_ = false;
}

/// Mission Component factory interface
Behavior* Pitch::CreateBehavior( const Str& prefix, const Module* module )
{
    return new Pitch( prefix, module );
}