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

#include "KeepStation.h"
#include "KeepStationIF.h"

#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"

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

/*
 * KeepStation tries to stay within a radius of a waypoint.
 * Initially the behavior will direct the vehicle to the
 * waypoint.  Once the waypoint has been reached the behavior
 * will direct the vehicle to have no speed.  Once the behavior
 * notices that it has gone out of the waypoint it will direct
 * the vehicle to go to the waypoint which, considering the
 * current, will let it "drift" the whole diameter of the circle.
 *
 */

KeepStation::KeepStation( const Str& prefix, const Module* module )
    : Behavior( prefix + KeepStationIF::NAME, module, true, true ),
      latitude_( nanf( "" ) ),
      longitude_( nanf( "" ) ),
      latitudeSetting_( nan( "" ) ),
      longitudeSetting_( nan( "" ) ),
      radiusSetting_( nan( "" ) ),
      speedSetting_( nan( "" ) ),
      mode_( NOT_INITIALIZED ),
      startLatitude_( nan( "" ) ),
      startLongitude_( nan( "" ) ),
      targetLatitude_( nan( "" ) ),
      targetLongitude_( nan( "" ) ),
      initialized_( false ),
      perpendicularSlope_( nanf( "" ) ),
      startOverLine_( false )
{
    logger_.syslog( "Construct KeepStation." );

    // Slate input setting variables
    latitudeSettingReader_ = newSettingReader( KeepStationIF::LATITUDE_SETTING );
    longitudeSettingReader_ = newSettingReader( KeepStationIF::LONGITUDE_SETTING );
    radiusSettingReader_ = newSettingReader( KeepStationIF::RADIUS_SETTING );
    speedSettingReader_ = newSettingReader( KeepStationIF::SPEED_SETTING );

    // Slate input measurements
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

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

KeepStation::~KeepStation()
{
}

void KeepStation::readSettings()
{
    // Configure settingLatitude_
    float latSetting( nanf( "" ) );
    if( latitudeSettingReader_->read( Units::RADIAN, latSetting ) && !isnan( latSetting ) )
    {
        if( latitudeSetting_ != latSetting )
        {
            latitudeSetting_ = latSetting;
            initialized_ = false;
        }
    }
    else
    {
        latitudeSetting_ = startLatitude_;
    }

    // Configure settingLongitude_
    float lonSetting( nanf( "" ) );
    if( longitudeSettingReader_->read( Units::RADIAN, lonSetting ) && !isnan( lonSetting ) )
    {
        if( longitudeSetting_ != lonSetting )
        {
            longitudeSetting_ = lonSetting;
            initialized_ = false;
        }
    }
    else
    {
        longitudeSetting_ = startLongitude_;
    }

    // Configure settingRadius_
    if( !radiusSettingReader_->read( Units::METER, radiusSetting_ ) )
    {
        radiusSetting_ = 500;
    }

    // Configure settingSpeed_
    if( !speedSettingReader_->read( Units::METER_PER_SECOND, speedSetting_ ) )
    {
        speedSetting_ = SpeedControlIF::SPEED_FAST;
    }


}

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

    latitudeSetting_ = nan( "" );
    longitudeSetting_ = nan( "" );
    radiusSetting_ = nan( "" );

    mode_ = NOT_INITIALIZED;

    if( !latitudeReader_->read( Units::RADIAN, startLatitude_ )
            || isnan( startLatitude_ )
            || !longitudeReader_->read( Units::RADIAN, startLongitude_ )
            || isnan( startLongitude_ ) )
    {
        initialized_ = false;
        return;
    }

    readSettings();

    mode_ = REST;
    targetLatitude_ = latitudeSetting_;
    targetLongitude_ = longitudeSetting_;
    initialized_ = true;
}

/// Just do the run: ignore the results of the satisfied
void KeepStation::run()
{
    bool sat( isSatisfied() );
    if( !initialized_ || NOT_INITIALIZED == mode_ )
    {
        initialize();
        return;
    }

    // Do we switch modes?
    switch( mode_ )
    {
    case GOTO_WAYPOINT:
        if( sat )
        {
            // We have reached our waypoint, switch mode to rest
            logger_.syslog( "Waypoint->Rest", Syslog::INFO );
            mode_ = REST;
        }
        break;
    case REST:
        if( !sat )
        {
            float distance = ( Location::GetDistance( targetLatitude_, targetLongitude_, latitude_, longitude_ ) );

            targetLatitude_ = latitudeSetting_;
            targetLongitude_ = longitudeSetting_;
            if( distance < 2 * radiusSetting_ )
            {
                // We are going out of the circle -- head to the other side.
                float targetBearing = Location::GetBearing( latitude_, longitude_, latitudeSetting_, longitudeSetting_ );
                Location::AtBearing( targetBearing, radiusSetting_ * 0.75, targetLatitude_, targetLongitude_ );
            }

            /// Slope of the line perpendicular between now and the waypoint
            if( targetLatitude_ != latitude_ )
            {
                perpendicularSlope_ = -( targetLongitude_ - longitude_ ) / ( targetLatitude_ - latitude_ );
                /// Is our starting point latitude > than the latitude of the starting
                /// longitude on the crossing line?
                startOverLine_ = latitude_ > targetLatitude_
                                 + ( longitude_ - targetLongitude_ ) * perpendicularSlope_;
            }
            else
            {
                perpendicularSlope_ = nan( "" );
                /// In this case, is the starting point longitude > than the setting longitude?
                startOverLine_ = longitude_ > targetLongitude_;
            }

            // We have a new target waypoint, switch mode to goto waypoint
            logger_.syslog( "Rest->Waypoint", Syslog::INFO );
            mode_ = GOTO_WAYPOINT;
        }
        break;
    default:
        logger_.syslog( "KeepStation reached unknown mode.", Syslog::ERROR );
        return;
    }

    // Depending on the mode we perform different actions
    switch( mode_ )
    {
    case GOTO_WAYPOINT: // XXX note that this does NOT use the waypoint behavior
        // Direct the vehicle to the target waypoint
        horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
        headingCmdWriter_->write( Units::RADIAN, Location::GetBearing( latitude_, longitude_, targetLatitude_, targetLongitude_ ) );
        speedCmdWriter_->write( Units::METER_PER_SECOND, speedSetting_ );
        break;
    case REST:
        // Instruct the vehicle to have no speed
        speedCmdWriter_->write( Units::METER_PER_SECOND, 0.0 );
        break;
    default:
        logger_.syslog( "KeepStation reached unknown mode.", Syslog::ERROR );
        return;
    }

}

/*
 * Depending on what mode we are in we have different
 * criteria for what the behavior considers satisfiable
 *
 * If resting, will return true if within the radius
 * of the specified waypoint
 *
 * If going to a waypoint, will return true if within
 * a capture radius of that waypoint
 *
 */
bool KeepStation::isSatisfied()
{
    readSettings();

    if( !initialized_ )
    {
        initialize();
    }
    if( !initialized_ )
    {
        return false;
    }

    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        latitudeReader_->requestData( true );
        longitudeReader_->requestData( true );
        return false;
    }

    // Get lat and lon
    latitudeReader_->read( Units::RADIAN, latitude_ );
    longitudeReader_->read( Units::RADIAN, longitude_ );
    if( isnan( latitude_ ) || isnan( longitude_ ) )
    {
        logger_.syslog( "Location is nan.", Syslog::ERROR );
        return false;
    }

    float distance;
    // Based on the mode, what satisfies this behavior?
    switch( mode_ )
    {
    case GOTO_WAYPOINT:
        // Test to see if the vehicle is within
        // distance of the target waypoint

        // Are we within 10% of the radius
        bool isOverLine;
        if( !isnan( perpendicularSlope_ ) )
        {
            isOverLine = latitude_ > targetLatitude_
                         + ( longitude_ - targetLongitude_ ) * perpendicularSlope_;
        }
        else
        {
            isOverLine = longitude_ > targetLongitude_;
        }
        return isOverLine != startOverLine_;
    case REST:
        // Test to see if the vehicle is within
        // the radius of latitudeSetting_ and
        // settingLongitude_
        distance = ( Location::GetDistance( latitudeSetting_, longitudeSetting_, latitude_, longitude_ ) );
        return  distance < radiusSetting_;
    default:
        logger_.syslog( "KeepStation reached unknown mode.", Syslog::ERROR );
        return false;
    }
}

/// Do the run, and return true if envelope "satisfied"
bool KeepStation::runIfUnsatisfied()
{
    // We will always be doing something
    run();
    return isSatisfied();
}

/// Uninit function
void KeepStation::uninitialize( void )
{
    logger_.syslog( "Uninitialize KeepStationComponent." );
    mode_ = NOT_INITIALIZED;
    initialized_ = false;
}

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