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

#include "Waypoint.h"
#include "WaypointIF.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"

Waypoint::Waypoint( const Str& prefix, const Module* module )
    : Behavior( prefix + WaypointIF::NAME, module, true, true ),
      latitudeSetting_( nan( "" ) ),
      longitudeSetting_( nan( "" ) ),
      bearing_( nan( "" ) ),
      captureRadiusSetting_( nan( "" ) ),
      waterFrameSetting_( false ),
      perpendicularSlope_( nan( "" ) ),
      startOverLine_( false ),
      endDistanceWrtSea_( nan( "" ) ),
      initialized_( false ),
      latlonSettingReal_( true )
{

    logger_.syslog( "Construct Waypoint." );

    // Slate input setting variables
    captureRadiusSettingReader_ = newSettingReader( WaypointIF::CAPTURE_RADIUS_SETTING );
    distanceDeltaSettingReader_ = newSettingReader( WaypointIF::DISTANCE_DELTA_SETTING );
    distanceDeltaBearingSettingReader_ = newSettingReader( WaypointIF::DISTANCE_DELTA_BEARING_SETTING );
    eastingsDeltaSettingReader_ = newSettingReader( WaypointIF::EASTINGS_DELTA_SETTING );
    latitudeSettingReader_ = newSettingReader( WaypointIF::LATITUDE_SETTING );
    latitudeDeltaSettingReader_ = newSettingReader( WaypointIF::LATITUDE_DELTA_SETTING );
    longitudeSettingReader_ = newSettingReader( WaypointIF::LONGITUDE_SETTING );
    longitudeDeltaSettingReader_ = newSettingReader( WaypointIF::LONGITUDE_DELTA_SETTING );
    northingsDeltaSettingReader_ = newSettingReader( WaypointIF::NORTHINGS_DELTA_SETTING );
    waterFrameSettingReader_ = newSettingReader( WaypointIF::WATER_FRAME_SETTING );

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

    // Slate output variables
    bearingCmdWriter_ = newDataWriter( HorizontalControlIF::BEARING_CMD );
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    latitudeCmdWriter_ = newDataWriter( HorizontalControlIF::LATITUDE_CMD );
    longitudeCmdWriter_ = newDataWriter( HorizontalControlIF::LONGITUDE_CMD );
}

Waypoint::~Waypoint()
{}

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

    latitudeSetting_ = nan( "" );
    longitudeSetting_ = nan( "" );
    captureRadiusSetting_ = nan( "" );
    waterFrameSetting_ = false ;
    latlonSettingReal_ = true;

    if( !latitudeSettingReader_->isActive()
            && !longitudeSettingReader_->isActive()
            && !latitudeDeltaSettingReader_->isActive()
            && !longitudeDeltaSettingReader_->isActive()
            && !eastingsDeltaSettingReader_->isActive()
            && !northingsDeltaSettingReader_->isActive()
            && !distanceDeltaSettingReader_->isActive()
            && !distanceDeltaBearingSettingReader_->isActive() )
    {
        logger_.syslog( "No Waypoint setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;
    }

    double startLatitude;
    double startLongitude;
    if( !latitudeReader_->isActive()
            || !latitudeReader_->read( Units::RADIAN, startLatitude )
            || isnan( startLatitude )
            || !longitudeReader_->isActive()
            || !longitudeReader_->read( Units::RADIAN, startLongitude )
            || isnan( startLongitude ) )
    {
        printf( "************** WP NOT INITIALIZED! \n" );
        return;
    }

    // Configure settingLatitude_
    if( !latitudeSettingReader_->isActive()
            || !latitudeSettingReader_->read( Units::RADIAN, latitudeSetting_ )
            || isnan( latitudeSetting_ ) )
    {
        latitudeSetting_ = startLatitude;
        latlonSettingReal_ = false;
    }
    float latitudeDelta;
    if( latitudeDeltaSettingReader_->isActive()
            &&	latitudeDeltaSettingReader_->read( Units::RADIAN, latitudeDelta )
            && !isnan( latitudeDelta ) )
    {
        latitudeSetting_ += latitudeDelta;
    }

    float northingsDeltaSetting( nan( "" ) );
    if( northingsDeltaSettingReader_->read( Units::METER, northingsDeltaSetting )
            && !isnan( northingsDeltaSetting ) )
    {
        latitudeSetting_ = Location::NorthingsDelta( latitudeSetting_, northingsDeltaSetting );
    }

    // Configure settingLongitude_
    if( ! longitudeSettingReader_->isActive()
            || !longitudeSettingReader_->read( Units::RADIAN, longitudeSetting_ )
            || isnan( longitudeSetting_ ) )
    {
        longitudeSetting_ = startLongitude;
        latlonSettingReal_ = false;
    }
    float longitudeDelta;
    if( longitudeDeltaSettingReader_->isActive()
            &&	longitudeDeltaSettingReader_->read( Units::RADIAN, longitudeDelta )
            && !isnan( longitudeDelta ) )
    {
        longitudeSetting_ += longitudeDelta;
    }
    float eastingsDeltaSetting( nan( "" ) );
    if( eastingsDeltaSettingReader_->read( Units::METER, eastingsDeltaSetting )
            && !isnan( eastingsDeltaSetting ) )
    {
        longitudeSetting_ = Location::EastingsDelta( latitudeSetting_, longitudeSetting_, eastingsDeltaSetting );
    }

    float distanceDeltaSetting( nanf( "" ) );
    if( distanceDeltaSettingReader_->isActive()
            && distanceDeltaSettingReader_->read( Units::METER, distanceDeltaSetting )
            && !isnan( distanceDeltaSetting ) )
    {
        float distanceDeltaBearingSetting( nanf( "" ) );
        if( !distanceDeltaBearingSettingReader_->isActive()
                || !distanceDeltaBearingSettingReader_->read( Units::RADIAN, distanceDeltaBearingSetting )
                || isnan( distanceDeltaBearingSetting ) )
        {
            logger_.syslog( "Must specify both distanceDelta and distanceDeltaBearing, or neither", Syslog::CRITICAL );
        }
        Location::AtBearing( distanceDeltaBearingSetting, distanceDeltaSetting, latitudeSetting_, longitudeSetting_ );
        // logger_.syslog( "go to (" + Str( R2D( latitudeSetting_ ), 4 ) + ", " + Str( R2D( longitudeSetting_ ), 4 ) + "), " + Str( distanceDeltaSetting, 1 ) + " m at bearing " + Str( R2D( distanceDeltaBearingSetting ), 1 ) + " degrees from (" + Str( R2D( startLatitude ), 4 ) + ", " + Str( R2D( startLongitude ), 4 ) + ")", Syslog::DEBUG );
    }

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

    if( captureRadiusSettingReader_->isActive() )
    {
        captureRadiusSettingReader_->read( Units::METER, captureRadiusSetting_ );
    }

    if( waterFrameSettingReader_->isActive() )
    {
        waterFrameSettingReader_->read( Units::BOOL, waterFrameSetting_ );
        if( waterFrameSetting_ == true )
        {
            if( !isnan( captureRadiusSetting_ ) )
            {
                logger_.syslog( "Should not set both waterFrame = true and captureRadius", Syslog::FAULT );
            }
            float startDistance;
            distanceWrtSeaReader_->read( Units::METER, startDistance );
            endDistanceWrtSea_ = startDistance
                                 + Location::GetDistance( startLatitude, startLongitude, latitudeSetting_, longitudeSetting_ );
        }
    }

    bearing_ = Location::GetBearing( startLatitude, startLongitude,
                                     latitudeSetting_, longitudeSetting_ );

    // Check to see if this was an actual commanded wpt as opposed to a nan from a mission before reporting it.
    if( latlonSettingReal_ )
    {
        logger_.syslog( "Navigating to waypoint: " + Str( R2D( latitudeSetting_ ) ) + "," + Str( R2D( longitudeSetting_ ) ), Syslog::IMPORTANT );
    }
    initialized_ = true;

}

bool Waypoint::isSatisfied()
{
    if( !initialized_ )
    {
        initialize();
    }
    if( !initialized_ )
    {
        return false;
    }

    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        //logger_.syslog( "Location Measurement is not Active.", SyslogSeverity::SYSLOG_ERROR );
        latitudeReader_->requestData( true );
        longitudeReader_->requestData( true );
        return false;
    }

    double latitude( nanf( "" ) );
    double longitude( nanf( "" ) );

    if( !latitudeReader_->read( Units::RADIAN, latitude ) || !longitudeReader_->read( Units::RADIAN, longitude ) )
    {
        logger_.syslog( "Location not readable.", Syslog::ERROR );
        return false;
    }

    if( isnan( latitude ) || isnan( longitude ) )
    {
        logger_.syslog( "Location is nan.", Syslog::ERROR );
        return false;
    }

    if( waterFrameSetting_ == true )
    {
        float distance( 0 );
        distanceWrtSeaReader_->read( Units::METER, distance );
        return distance >= endDistanceWrtSea_;

    }

    if( !isnan( captureRadiusSetting_ ) )
    {
        float distance( Location::GetDistance( latitudeSetting_, longitudeSetting_, latitude, longitude ) );
        return distance <= captureRadiusSetting_;
    }

    bool isOverLine;
    if( !isnan( perpendicularSlope_ ) )
    {
        isOverLine = latitude > latitudeSetting_
                     + ( longitude - longitudeSetting_ ) * perpendicularSlope_;
    }
    else
    {
        isOverLine = longitude > longitudeSetting_;
    }

    return isOverLine != startOverLine_;
}

bool Waypoint::runIfUnsatisfied()
{
    //logger_.syslog( "Running Waypoint." );

    if( isSatisfied() )
    {
        // Check to see if this was an actual commanded wpt as opposed to a nan from a mission before reporting it.
        if( latlonSettingReal_ )
        {
            logger_.syslog( "Reached waypoint: " + Str( R2D( latitudeSetting_ ) ) + "," + Str( R2D( longitudeSetting_ ) ), Syslog::IMPORTANT );
        }
        return true;
    }

    run();

    return false;
}

/// The actual "payload" of the component
void Waypoint::run()
{
    // Set the waypoint / heading
    horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::WAYPOINT );
    bearingCmdWriter_->write( Units::RADIAN, bearing_ );
    if( waterFrameSetting_ == false )
    {
        latitudeCmdWriter_->write( Units::RADIAN, latitudeSetting_ );
        longitudeCmdWriter_->write( Units::RADIAN, longitudeSetting_ );
    }
}

/// Uninit function
void Waypoint::uninitialize( void )
{
    logger_.syslog( "Uninitialize WaypointComponent." );
    initialized_ = false;
    //outputDepth_ ->setActive( false );
    //outputDepthMode_ ->setActive( false );
}

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