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

#include "ZigZag.h"
#include "ZigZagIF.h"

#include "controlModule/HorizontalControlIF.h"

#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

ZigZag::ZigZag( const Str& prefix, const Module* module )
    : Behavior( prefix + ZigZagIF::NAME, module, true, true ),
      bearingSetting_( nan( "" ) ),
      angleSetting_( nan( "" ) ),
      headingRead_( nan( "" ) ),
      headingCmd_( nan( "" ) ),
      initialized_( false ),
      goingRight_( true ),
      satisfied_( false ),
      ranOnce_( false )
{

    logger_.syslog( "Construct ZigZag." );

    // Slate input setting variables
    bearingSettingReader_ = newSettingReader( ZigZagIF::BEARING_SETTING );
    angleSettingReader_ = newSettingReader( ZigZagIF::ANGLE_SETTING );

    // Slate input measurements
    horizontalModeReader_ = newDataReader( HorizontalControlIF::HORIZONTAL_MODE ); //, this, Units::ENUM() );
    headingCmdReader_ = newDataReader( HorizontalControlIF::HEADING_CMD ); //, this, Units::RADIAN() );
    bearingCmdReader_ = newDataReader( HorizontalControlIF::BEARING_CMD ); //, this, Units::RADIAN() );

    // Slate output variables
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    headingCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD );
}

ZigZag::~ZigZag()
{}

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

    bearingSetting_ = nan( "" );
    angleSetting_ = nan( "" );

    if( !bearingSettingReader_->isActive() )
    {
        logger_.syslog( "Missing bearing setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;
    }

    if( !angleSettingReader_->isActive() )
    {
        logger_.syslog( "Missing angle setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;
    }

    readSettings( true );

    initialized_ = true;

    goingRight_ = true;

}

bool ZigZag::isSatisfied()
{
    return calculate() && calcSatisfied();
}

void ZigZag::run()
{
    if( calculate() )
    {
        writeCmds();
    }
}

bool ZigZag::runIfUnsatisfied()
{
    if( calculate() )
    {
        if( satisfied_ )
        {
            logger_.syslog( "Reached end of zig or zag", Syslog::INFO );
        }
        writeCmds();
    }
    return satisfied_;
}

void ZigZag::readSettings( bool force )
{
    // Read settings
    if( force || bearingSettingReader_->isActive() )
    {
        bearingSettingReader_->read( Units::RADIAN, bearingSetting_ );
    }
    if( force || angleSettingReader_->isActive() )
    {
        angleSettingReader_->read( Units::METER, angleSetting_ );
    }
}

bool ZigZag::calculate()
{
    if( !initialized_ )
    {
        return false;
    }

    readSettings( false );

    headingRead_ = nanf( "" );
    int headingMode;
    if( horizontalModeReader_->isActive()
            && horizontalModeReader_->wasTouchedSinceLastRun( this )
            && horizontalModeReader_->read( Units::ENUM, headingMode )
            && ( headingMode == HorizontalControlIF::HEADING
                 || headingMode == HorizontalControlIF::WAYPOINT ) )
    {
        if( headingMode == HorizontalControlIF::HEADING )
        {
            headingCmdReader_->read( Units::RADIAN, headingRead_ );
        }
        else if( headingMode == HorizontalControlIF::WAYPOINT )
        {
            bearingCmdReader_->read( Units::RADIAN, headingRead_ );
        }
    }

    if( !isnan( headingRead_ ) )
    {
        float err = AuvMath::ModPi( headingRead_ - bearingSetting_ );
        if( err > 0 )
        {
            setGoingRight( true );
        }
        else if( err < 0 )
        {
            setGoingRight( false );
        }
    }

    if( goingRight_ )
    {
        headingCmd_ = bearingSetting_ + angleSetting_;
    }
    else
    {
        headingCmd_ = bearingSetting_ - angleSetting_;
    }

    ranOnce_ = true;

    return true;
}

void ZigZag::setGoingRight( bool goingRight )
{
    satisfied_ = goingRight_ != goingRight && ranOnce_;
    goingRight_ = goingRight;
}

bool ZigZag::calcSatisfied()
{

    return satisfied_;
}

///
void ZigZag::writeCmds()
{
    horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
    headingCmdWriter_->write( Units::RADIAN, headingCmd_ );
}

/// Uninit function
void ZigZag::uninitialize( void )
{
    logger_.syslog( "Uninitialize ZigZagComponent." );
    initialized_ = false;
}

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