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

#include "Point.h"
#include "PointIF.h"

#include "controlModule/HorizontalControlIF.h"

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

#include <math.h>

Point::Point( const Str& prefix, const Module* module )
    : Behavior( prefix + PointIF::NAME, module, true, true ),
      /// Desired latitude of the vehicle
      latitudeSetting_( nan( "" ) ),
      /// Desired longitude of the vehicle
      longitudeSetting_( nan( "" ) ),
      /// Desired orientation of the vehicle
      headingSetting_( nan( "" ) ),
      /// Desired yaw rate of the vehicle
      headingRateSetting_( nan( "" ) ),
      /// Desired rudder angle for the vehicle.
      rudderAngleSetting_( nan( "" ) ),
      /// Initial latitude of the vehicle
      initialLatitude_( nan( "" ) ),
      /// Initial longitude of the vehicle
      initialLongitude_( nan( "" ) ),
      /// Initial orientation of the vehicle
      initialOrientation_( nan( "" ) ),
      /// Current Latitude of the vehicle
      latitude_( nan( "" ) ),
      /// Current Longitude of the vehicle
      longitude_( nan( "" ) ),
      /// Current Orientation of the vehicle
      orientation_( nan( "" ) ),
      /// Last orientation delta of the vehicle
      lastOrientationDelta_( nan( "" ) ),
      /// Slope for crossing line (thru wapoint)
      perpendicularSlope_( nan( "" ) ),
      /// Is our starting point latitude > than the latitude of the starting
      /// longitude on the crossing line?
      startOverLine_( false ),
      /// Force update of input settings
      forceUpdate_( false ),
      /// Have we learned all our initial settings?
      initialized_( false )

{
    logger_.syslog( "Construct." );

    // Slate input setting variables
    headingSettingReader_ = newSettingReader( PointIF::HEADING_SETTING );
    headingDeltaSettingReader_ = newSettingReader( PointIF::HEADING_DELTA_SETTING );
    headingRateSettingReader_ = newSettingReader( PointIF::HEADING_RATE_SETTING );
    rudderAngleSettingReader_ = newSettingReader( PointIF::RUDDER_ANGLE_SETTING );
    latitudeSettingReader_ = newSettingReader( PointIF::LATITUDE_SETTING );
    latitudeDeltaSettingReader_ = newSettingReader( PointIF::LATITUDE_DELTA_SETTING );
    northingsDeltaSettingReader_ = newSettingReader( PointIF::NORTHINGS_DELTA_SETTING );
    longitudeSettingReader_ = newSettingReader( PointIF::LONGITUDE_SETTING );
    longitudeDeltaSettingReader_ = newSettingReader( PointIF::LONGITUDE_DELTA_SETTING );
    eastingsDeltaSettingReader_ = newSettingReader( PointIF::EASTINGS_DELTA_SETTING );
    forceUpdateSettingReader_ = newSettingReader( PointIF::FORCE_UPDATE_SETTING );

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

    // Slate output variables
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    headingCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD );
    headingRateCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_RATE_CMD );
    rudderAngleCmdWriter_ = newDataWriter( HorizontalControlIF::RUDDER_ANGLE_CMD );
    latitudeCmdWriter_ = newDataWriter( HorizontalControlIF::LATITUDE_CMD );
    longitudeCmdWriter_ = newDataWriter( HorizontalControlIF::LONGITUDE_CMD );
}

Point::~Point()
{}

/// Initialize function
void Point::initialize()
{
    if( !forceUpdate_ ) logger_.syslog( "Initialize." );

    latitudeSetting_ = nan( "" );
    longitudeSetting_ =  nan( "" );
    headingSetting_ =  nan( "" );
    headingRateSetting_ =  nan( "" );
    rudderAngleSetting_ = nan( "" );

    orientationReader_->requestData( true );

    // Let's read in all the relevant dataReaders
    float headingSetting( nan( "" ) );
    headingSettingReader_->read( Units::RADIAN, headingSetting );
    if( !isnan( headingSetting ) )
    {
        headingSetting_ = headingSetting;
    }

    float headingDeltaSetting( nan( "" ) );
    headingDeltaSettingReader_->read( Units::RADIAN, headingDeltaSetting );
    if( !isnan( headingDeltaSetting ) )
    {
        if( isnan( headingSetting_ ) )
        {
            orientationReader_->read( Units::RADIAN, orientation_ );
            if( isnan( orientation_ ) )
            {
                return; // leave initialized_ = false_;
            }
            headingSetting_ = orientation_;
        }
        headingSetting_ = AuvMath::ModPi( headingSetting_ + headingDeltaSetting );
    }
    if( !isnan( headingSetting_ ) )
    {
        orientationReader_->read( Units::RADIAN, initialOrientation_ );
        if( isnan( initialOrientation_ ) )
        {
            return; // leave initialized_ = false_;
        }

    }

    headingRateSettingReader_->read( Units::RADIAN_PER_SECOND, headingRateSetting_ );
    rudderAngleSettingReader_->read( Units::RADIAN, rudderAngleSetting_ );

    latitudeSettingReader_->read( Units::RADIAN, latitudeSetting_ );
    float latitudeDeltaSetting( nan( "" ) );
    latitudeDeltaSettingReader_->read( Units::RADIAN, latitudeDeltaSetting );
    float northingsDeltaSetting( nan( "" ) );
    northingsDeltaSettingReader_->read( Units::METER, northingsDeltaSetting );
    if( isnan( latitudeSetting_ ) )
    {
        if( !isnan( latitudeDeltaSetting ) || !isnan( northingsDeltaSetting ) )
        {
            latitudeReader_->read( Units::RADIAN, latitude_ );
            if( isnan( latitude_ ) )
            {
                return; // leave initialized_ = false_;
            }
            latitudeSetting_ = latitude_;
        }
    }
    if( !isnan( latitudeDeltaSetting ) )
    {
        latitudeSetting_ += latitudeDeltaSetting;
    }
    if( !isnan( northingsDeltaSetting ) )
    {
        latitudeSetting_ = Location::NorthingsDelta( latitudeSetting_, northingsDeltaSetting );
    }
    if( !isnan( latitudeSetting_ ) )
    {
        latitudeReader_->read( Units::RADIAN, initialLatitude_ );
        if( isnan( initialLatitude_ ) )
        {
            return; // leave initialized_ = false_;
        }
    }

    longitudeSettingReader_->read( Units::RADIAN, longitudeSetting_ );
    float longitudeDeltaSetting( nan( "" ) );
    longitudeDeltaSettingReader_->read( Units::RADIAN, longitudeDeltaSetting );
    float eastingsDeltaSetting( nan( "" ) );
    eastingsDeltaSettingReader_->read( Units::METER, eastingsDeltaSetting );
    if( isnan( longitudeSetting_ ) )
    {
        if( !isnan( longitudeDeltaSetting ) || !isnan( eastingsDeltaSetting ) )
        {
            longitudeReader_->read( Units::RADIAN, longitude_ );
            if( isnan( longitude_ ) )
            {
                return; // leave initialized_ = false_;
            }
            longitudeSetting_ = longitude_;
        }
    }
    if( !isnan( longitudeDeltaSetting ) )
    {
        longitudeSetting_ += longitudeDeltaSetting;
    }
    if( !isnan( eastingsDeltaSetting ) )
    {
        latitudeReader_->read( Units::RADIAN, latitude_ );
        if( isnan( latitude_ ) )
        {
            return; // leave initialized_ = false_;
        }
        longitudeSetting_ = Location::EastingsDelta( latitude_, longitudeSetting_, eastingsDeltaSetting );
    }
    if( !isnan( longitudeSetting_ ) )
    {
        longitudeReader_->read( Units::RADIAN, initialLongitude_ );
        if( isnan( initialLongitude_ ) )
        {
            return; // leave initialized_ = false_;
        }
    }

    if( !isnan( latitudeSetting_ ) && !isnan( longitudeSetting_ ) )
    {
        if( latitudeSetting_ != initialLatitude_ )
        {
            perpendicularSlope_ = -( longitudeSetting_ - initialLongitude_ ) / ( latitudeSetting_ - initialLatitude_ );
            /// Is our starting point latitude > than the latitude of the starting
            /// longitude on the crossing line?
            startOverLine_ = initialLatitude_ > latitudeSetting_
                             + ( initialLongitude_ - longitudeSetting_ ) * perpendicularSlope_;
        }
        else
        {
            perpendicularSlope_ = nan( "" );
            /// In this case, is the starting point longitude > than the setting longitude?
            startOverLine_ = initialLongitude_ > longitudeSetting_;
        }

    }

    lastOrientationDelta_ = nan( "" );
    //printf("settingBearing_=%g,settingLatitude_=%g,settingLongitude_=%g,settingRudderAngle_=%g,settingYawRate_=%g,settingLongitudeDelta=%g\n",R2D(bearingSetting_),R2D(latitudeSetting_),R2D(longitudeSetting_),R2D(rudderAngleSetting_),R2D(yawRateSetting_),R2D(longitudeDeltaSetting));

    if( !forceUpdateSettingReader_->read( Units::BOOL, forceUpdate_ ) )
    {
        forceUpdate_ = 0;
    }
    initialized_ = true;
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.

bool Point::readParams()
{
    if( !initialized_ || forceUpdate_ )
    {
        initialize();
    }
    if( !initialized_ )
    {
        return false;
    }

    if( !isnan( latitudeSetting_ ) )
    {
        latitudeReader_->read( Units::RADIAN, latitude_ );
        if( isnan( latitude_ ) )
        {
            return false;
        }
    }

    if( !isnan( longitudeSetting_ ) )
    {
        longitudeReader_->read( Units::RADIAN, longitude_ );
        if( isnan( longitude_ ) )
        {
            return false;
        }
    }

    if( !isnan( headingSetting_ ) )
    {
        if( isnan( latitudeSetting_ ) && isnan( longitudeSetting_ ) )
        {
            orientationReader_->read( Units::RADIAN, orientation_ );
            if( isnan( orientation_ ) )
            {
                return false;
            }
        }
    }

    return true;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool Point::calcSatisfied()
{
    if( !isnan( latitudeSetting_ ) && !isnan( longitudeSetting_ ) )
    {
        bool isOverLine;
        if( !isnan( perpendicularSlope_ ) )
        {
            isOverLine = latitude_ > latitudeSetting_
                         + ( longitude_ - longitudeSetting_ ) * perpendicularSlope_;
        }
        else
        {
            isOverLine = longitude_ > longitudeSetting_;
        }
        return isOverLine != startOverLine_;
    }

    if( !isnan( latitudeSetting_ ) )
    {
        return ( latitudeSetting_ > initialLatitude_ ) ?
               ( latitude_ >= latitudeSetting_ ) :
               ( latitude_ <= latitudeSetting_ );
    }

    if( !isnan( longitudeSetting_ ) )
    {
        return ( longitudeSetting_ > initialLongitude_ ) ?
               ( longitude_ >= longitudeSetting_ ) :
               ( longitude_ <= longitudeSetting_ );
    }

    if( !isnan( headingSetting_ ) )
    {
        float orientationDelta = AuvMath::ModPi( orientation_ - headingSetting_ );
        bool satisfied = fabs( orientationDelta ) < 0.01 ||
                         ( fabs( orientationDelta ) < M_PI_2 &&
                           fabs( lastOrientationDelta_ ) < M_PI_2 &&
                           AuvMath::Sign( orientationDelta ) != AuvMath::Sign( lastOrientationDelta_ ) );
        lastOrientationDelta_ = orientationDelta;
        return satisfied;
    }

    return false;
}

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

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

/// Do the run, and return true if envelope "satisfied"
bool Point::runIfUnsatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        satisfied = calcSatisfied();
        if( !satisfied )
        {
            if( !isnan( headingRateSetting_ ) )
            {
                headingRateCmdWriter_->write( Units::RADIAN, headingRateSetting_ );
            }
            if( !isnan( rudderAngleSetting_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
                rudderAngleCmdWriter_->write( Units::RADIAN, rudderAngleSetting_ );
            }
            else if( !isnan( headingSetting_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
                headingCmdWriter_->write( Units::RADIAN, headingSetting_ );
            }
            else if( !isnan( latitudeSetting_ ) && !isnan( longitudeSetting_ ) )
            {
                float bearing = Location::GetBearing( latitude_, longitude_, latitudeSetting_, longitudeSetting_ );
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
                headingCmdWriter_->write( Units::RADIAN, bearing );
            }
            else if( !isnan( latitudeSetting_ ) )
            {
                float bearing = latitude_ < latitudeSetting_ ? 0 : M_PI;
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
                headingCmdWriter_->write( Units::RADIAN, bearing );
            }
            else if( !isnan( longitudeSetting_ ) )
            {
                float bearing = longitude_ < longitudeSetting_ ? M_PI_2 : -M_PI_2;
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
                headingCmdWriter_->write( Units::RADIAN, bearing );
            }
            else if( !isnan( headingRateSetting_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING_RATE );
            }
        }
        else
        {
            if( !isnan( headingRateSetting_ ) )
            {
                headingRateCmdWriter_->write( Units::RADIAN, headingRateSetting_ );
            }
            if( !isnan( rudderAngleSetting_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::RUDDER_ANGLE );
                rudderAngleCmdWriter_->write( Units::RADIAN, rudderAngleSetting_ );
            }
            else if( !isnan( headingSetting_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
                headingCmdWriter_->write( Units::RADIAN, headingSetting_ );
            }
            else if( !isnan( latitudeSetting_ ) || !isnan( longitudeSetting_ ) )
            {
                // Do nothing
            }
            else if( !isnan( headingRateSetting_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING_RATE );
            }
        }
    }
    return satisfied;
}

/// Uninit function
void Point::uninitialize()
{
    orientationReader_->requestData( false );
}

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