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

#include "Circle.h"
#include "CircleIF.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"

Circle::Circle( const Str& prefix, const Module* module )
    : Behavior( prefix + CircleIF::NAME, module, true, true ),
      startAngle_( nanf( "" ) ),
      thresholdAngle_( nanf( "" ) ),
      passedThreshold_( false ),
      angleSetting_( nanf( "" ) ),
      latitudeSetting_( nanf( "" ) ),
      longitudeSetting_( nanf( "" ) ),
      maxErrorSetting_( nanf( "" ) ),
      radiusSetting_( 500 ),
      turnToPortSetting_( true ),
      initialized_( false )
{

    logger_.syslog( "Construct." );

    // Slate input setting variables
    angleSettingReader_ = newSettingReader( CircleIF::ANGLE_SETTING );
    eastingsDeltaSettingReader_ = newSettingReader( CircleIF::EASTINGS_DELTA_SETTING );
    latitudeSettingReader_ = newSettingReader( CircleIF::LATITUDE_SETTING );
    latitudeDeltaSettingReader_ = newSettingReader( CircleIF::LATITUDE_DELTA_SETTING );
    longitudeSettingReader_ = newSettingReader( CircleIF::LONGITUDE_SETTING );
    longitudeDeltaSettingReader_ = newSettingReader( CircleIF::LONGITUDE_DELTA_SETTING );
    maxErrorSettingReader_ = newSettingReader( CircleIF::MAX_ERROR_SETTING );
    northingsDeltaSettingReader_ = newSettingReader( CircleIF::NORTHINGS_DELTA_SETTING );
    radiusSettingReader_ = newSettingReader( CircleIF::RADIUS_SETTING );
    turnToPortSettingReader_ = newSettingReader( CircleIF::TURN_TO_PORT_SETTING );

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

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

Circle::~Circle()
{}

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

    angleSetting_ = nanf( "" );
    latitudeSetting_ = nanf( "" );
    longitudeSetting_ = nanf( "" );
    maxErrorSetting_ = nanf( "" );
    radiusSetting_ = 500;
    turnToPortSetting_ = true;

    if( !radiusSettingReader_->isActive() )
    {
        logger_.syslog( "No Radius setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;
    }

    if( !turnToPortSettingReader_->isActive() )
    {
        logger_.syslog( "No Turn to port setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;
    }

    float startLatitude;
    float startLongitude;
    if( !latitudeReader_->isActive()
            || !latitudeReader_->read( Units::RADIAN, startLatitude )
            || isnan( startLatitude )
            || !longitudeReader_->isActive()
            || !longitudeReader_->read( Units::RADIAN, startLongitude )
            || isnan( startLongitude ) )
    {
        return;
    }

    // Configure settingLatitude_
    if( latitudeSettingReader_->isActive() )
    {
        latitudeSettingReader_->read( Units::RADIAN, latitudeSetting_ );
    }
    else
    {
        latitudeSetting_ = startLatitude;
    }
    float latitudeDelta;
    if( latitudeDeltaSettingReader_->isActive()
            &&	latitudeDeltaSettingReader_->read( Units::RADIAN, 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_ );
    }
    else
    {
        longitudeSetting_ = startLongitude;
    }
    float longitudeDelta;
    if( longitudeDeltaSettingReader_->isActive()
            &&	longitudeDeltaSettingReader_->read( Units::RADIAN, longitudeDelta ) )
    {
        longitudeSetting_ += longitudeDelta;
    }
    float eastingsDeltaSetting( nan( "" ) );
    if( eastingsDeltaSettingReader_->read( Units::METER, eastingsDeltaSetting )
            && !isnan( eastingsDeltaSetting ) )
    {
        longitudeSetting_ = Location::EastingsDelta( latitudeSetting_, longitudeSetting_, eastingsDeltaSetting );
    }

    if( radiusSettingReader_->isActive() )
    {
        radiusSettingReader_->read( Units::METER, radiusSetting_ );
    }

    if( turnToPortSettingReader_->isActive() )
    {
        int turnToPortInt( 0 );
        turnToPortSettingReader_->read( Units::BOOL, turnToPortInt );
        turnToPortSetting_ = ( 0 != turnToPortInt );
    }

    if( angleSettingReader_->isActive() )
    {
        angleSettingReader_->read( Units::RADIAN, angleSetting_ );
    }
    else
    {
        angleSetting_ = M_2PI;
    }

    if( maxErrorSettingReader_->isActive() )
    {
        maxErrorSettingReader_->read( Units::METER, maxErrorSetting_ );
    }
    else
    {
        maxErrorSetting_ = 50;
    }

    thresholdAngle_ = fabs( AuvMath::Min( angleSetting_, ( float )M_PI_4 ) );

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

    passedThreshold_ = false;

    initialized_ = true;
}

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

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

    double latitude;
    double longitude;
    latitudeReader_->read( Units::RADIAN, latitude );
    longitudeReader_->read( Units::RADIAN, longitude );

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

    float angle = Location::GetBearing( latitudeSetting_, longitudeSetting_,  latitude, longitude );
    float dAngle = AuvMath::ModPi( angle - startAngle_ );
    if( !passedThreshold_ )
    {
        passedThreshold_ = ( turnToPortSetting_ && dAngle < -thresholdAngle_ )
                           || ( !turnToPortSetting_ && dAngle > thresholdAngle_ );
    }

    if( !passedThreshold_ )
    {
        return false;
    }

    if( ( turnToPortSetting_ && dAngle > -thresholdAngle_ && dAngle < 0 )
            || ( !turnToPortSetting_ && dAngle < thresholdAngle_ && dAngle > 0 ) )
    {
        passedThreshold_ = false;
        return true;
    }

    return false;
}

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

    if( isSatisfied() )
    {
        return true;
    }

    run();

    return false;
}

/// The actual "payload" of the component
void Circle::run()
{

    if( latitudeSettingReader_->isActive() )
    {
        latitudeSettingReader_->read( Units::RADIAN, latitudeSetting_ );

        float latitudeDelta;
        if( latitudeDeltaSettingReader_->isActive()
                &&  latitudeDeltaSettingReader_->read( Units::RADIAN, 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_ );
        float longitudeDelta;
        if( longitudeDeltaSettingReader_->isActive()
                &&  longitudeDeltaSettingReader_->read( Units::RADIAN, longitudeDelta ) )
        {
            longitudeSetting_ += longitudeDelta;
        }
        float eastingsDeltaSetting( nan( "" ) );
        if( eastingsDeltaSettingReader_->read( Units::METER, eastingsDeltaSetting )
                && !isnan( eastingsDeltaSetting ) )
        {
            longitudeSetting_ = Location::EastingsDelta( latitudeSetting_, longitudeSetting_, eastingsDeltaSetting );
        }
    }

    if( radiusSettingReader_->isActive() )
    {
        radiusSettingReader_->read( Units::METER, radiusSetting_ );
    }

    if( turnToPortSettingReader_->isActive() )
    {
        int turnToPortInt( 0 );
        turnToPortSettingReader_->read( Units::BOOL, turnToPortInt );
        turnToPortSetting_ = ( 0 != turnToPortInt );
    }

    if( angleSettingReader_->isActive() )
    {
        angleSettingReader_->read( Units::RADIAN, angleSetting_ );
    }

    if( maxErrorSettingReader_->isActive() )
    {
        maxErrorSettingReader_->read( Units::METER, maxErrorSetting_ );
    }

    double latitude;
    double longitude;
    latitudeReader_->read( Units::RADIAN, latitude );
    longitudeReader_->read( Units::RADIAN, longitude );

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

    // Get our distance from the center of the circle
    float distance( Location::GetDistance( latitudeSetting_, longitudeSetting_,
                                           latitude, longitude ) );

    // Get our angle from the center of the circle
    float angle( Location::GetBearing( latitudeSetting_, longitudeSetting_,
                                       latitude, longitude ) );
    float bearing( nanf( "" ) );

    if( distance > radiusSetting_ + maxErrorSetting_ )
    {
        // Head straight for the center of the circle
        bearing = AuvMath::ModPi( angle + M_PI );
        latitude = nanf( "" );
        longitude = nanf( "" );
    }
    else if( distance < radiusSetting_ - maxErrorSetting_ )
    {
        // Head straight from the center of the circle
        bearing = angle;
        latitude = nanf( "" );
        longitude = nanf( "" );
    }
    else
    {
        latitude = latitudeSetting_;
        longitude = longitudeSetting_;
        // Move point to the edge of the circle
        Location::AtBearing( angle, radiusSetting_, latitude, longitude );
        // Get the tangental bearing
        bearing = angle + M_PI_2 * ( turnToPortSetting_ ? -1.0f : 1.0f );
    }
    // Set the waypoint / heading
    horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::WAYPOINT );
    bearingCmdWriter_->write( Units::RADIAN, bearing );
    latitudeCmdWriter_->write( Units::RADIAN, latitude );
    longitudeCmdWriter_->write( Units::RADIAN, longitude );
}

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

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