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

#include "OffshoreEnvelope.h"
#include "OffshoreEnvelopeIF.h"

#include "controlModule/SpeedControlIF.h"
#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "io/NavChartDb.h"
#include "utils/AuvMath.h"
#include "units/Units.h"

OffshoreEnvelope::OffshoreEnvelope( const Str& prefix, const Module* module )
    : Behavior( prefix + OffshoreEnvelopeIF::NAME, module, true, true ),
      lastTimeInEnvelope_( Timestamp::NOT_SET_TIME )
{

    logger_.syslog( "Construct OffshoreEnvelope." );

    // Slate input setting variables
    minOffshoreSettingReader_ = newSettingReader( OffshoreEnvelopeIF::MIN_OFFSHORE_SETTING );
    maxOffshoreSettingReader_ = newSettingReader( OffshoreEnvelopeIF::MAX_OFFSHORE_SETTING );

    /// Slate input setting variables on their way to DynamicControl
    horizontalModeReader_ = newDataReader( HorizontalControlIF::HORIZONTAL_MODE );
    latitudeCmdReader_ = newDataReader( HorizontalControlIF::LATITUDE_CMD );
    longitudeCmdReader_ = newDataReader( HorizontalControlIF::LONGITUDE_CMD );
    bearingCmdReader_ = newDataReader( HorizontalControlIF::BEARING_CMD );
    headingCmdReader_ = newDataReader( HorizontalControlIF::HEADING_CMD );
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD );

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

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

}

OffshoreEnvelope::~OffshoreEnvelope()
{}

/// Initialize function
void OffshoreEnvelope::initialize( void )
{
    logger_.syslog( "Initialize OffshoreEnvelopeComponent." );
    offshoreDistanceReader_->requestData( true );
    orientationReader_->requestData( true );
    lastTimeInEnvelope_ = Timestamp::Now();
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool OffshoreEnvelope::readParams( float &minOffshore, float& maxOffshore,
                                   HorizontalControlIF::HorizontalMode& horizontalMode, float& speed,
                                   float& latitude, float& longitude, float& offshoreDistance )
{
    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        logger_.syslog( "Location Measurement is not Active.", Syslog::ERROR );
        return false;
    }

    // Let's read in all the relevant readerSettings
    bool gotMin = minOffshoreSettingReader_->read( Units::METER, minOffshore );
    bool gotMax = maxOffshoreSettingReader_->read( Units::METER, maxOffshore );
    if( !gotMin && !gotMax )
    {
        logger_.syslog( "Neither min nor max setting specified.", Syslog::CRITICAL );
    }

    horizontalMode = ( HorizontalControlIF::HorizontalMode )( horizontalModeReader_->asInt( Units::ENUM ) );
// TODO: Come back and check this functionality, then clean it up.
//    int hMode;
//    horizontalModeReader_->read( Units::ENUM, hMode );
//    horizontalMode = ( HorizontalControlIF::HorizontalMode )hmode;

    speedCmdReader_->read( Units::METER_PER_SECOND, speed ); // TODO: Why are we passing speed as an arg rather than using a member parameter?

    // Let's read in all the relevant measurements
    bool ok( true );
    ok &= offshoreDistanceReader_->read( Units::METER, offshoreDistance );
    ok &= latitudeReader_->read( Units::RADIAN, latitude );
    ok &= longitudeReader_->read( Units::RADIAN, longitude );
    return ok;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool OffshoreEnvelope::calcSatisfied( const float minOffshore, const float maxOffshore,
                                      const float offshoreDistance )
{

    // Now we get to work, iff location is valid!...
    return isnan( offshoreDistance ) ||
           ( ( isnan( minOffshore ) || offshoreDistance >= minOffshore ) &&
             ( isnan( maxOffshore ) || offshoreDistance <= maxOffshore ) );

}

/// Just do the run: ignore the results of the satisfied
void OffshoreEnvelope::run()
{
    runIfUnsatisfied();
}

/// Just do the satisfied: return true if envelope "satisfied"
bool OffshoreEnvelope::isSatisfied()
{
    float minOffshore( nanf( "" ) );
    float maxOffshore( nanf( "" ) );
    HorizontalControlIF::HorizontalMode horizontalMode( HorizontalControlIF::NONE );
    float speed( nanf( "" ) );
    float latitude( nanf( "" ) );
    float longitude( nanf( "" ) );
    float offshoreDistance( nanf( "" ) );

    if( readParams( minOffshore, maxOffshore, horizontalMode, speed, latitude, longitude, offshoreDistance ) )
    {
        return calcSatisfied( minOffshore, maxOffshore, offshoreDistance );
    }
    return false;
}

/// Do the run, and return true if envelope "satisfied"
bool OffshoreEnvelope::runIfUnsatisfied()
{
    //logger_.syslog( "Running OffshoreEnvelope." );

    float minOffshore( nanf( "" ) );
    float maxOffshore( nanf( "" ) );
    HorizontalControlIF::HorizontalMode horizontalMode( HorizontalControlIF::NONE );
    float speedCmd( nanf( "" ) );
    float latitude( nanf( "" ) );
    float longitude( nanf( "" ) );
    float offshoreDistance( nanf( "" ) );

    if( readParams( minOffshore, maxOffshore, horizontalMode, speedCmd, latitude, longitude, offshoreDistance ) )
    {
        if( isnan( latitude ) || isnan( longitude ) || isnan( offshoreDistance ) )
        {
            return false;
        }

        if( !calcSatisfied( minOffshore, maxOffshore, offshoreDistance ) )
        {
            if( lastTimeInEnvelope_.elapsed() > Timespan( 1800 ) )
            {
                logger_.syslog( "Offshore envelope ACTIVE", Syslog::IMPORTANT );
                lastTimeInEnvelope_ = Timestamp::Now();
            }

            // Need to move! -- so make sure we have some speed.
            if( isnan( speedCmd ) || speedCmd < SpeedControlIF::SPEED_FAST ) // TODO: Shouldn't need the isnan check anymore, but want to test thoroughly before removing it.
            {
                speedCmd = SpeedControlIF::SPEED_FAST;
            }
            speedCmdWriter_->write( Units::METER_PER_SECOND, speedCmd );

            // Now figure out our current heading
            float headingCmd = nanf( "" );
            float bearingCmd = nanf( "" );
            double latitudeCmd = nanf( "" );
            double longitudeCmd = nanf( "" );
            switch( horizontalMode )
            {
            case HorizontalControlIF::HEADING:
                headingCmdReader_->read( Units::RADIAN, headingCmd );
                break;
            case HorizontalControlIF::WAYPOINT:
                latitudeCmdReader_->read( Units::RADIAN, latitudeCmd );
                longitudeCmdReader_->read( Units::RADIAN, longitudeCmd );
                if( !isnan( latitudeCmd ) && !isnan( latitudeCmd ) )
                {
                    headingCmd = Location::GetBearing( latitude, longitude, latitudeCmd, longitudeCmd );
                }
                if( bearingCmdReader_->read( Units::RADIAN, bearingCmd ) && !isnan( bearingCmd ) )
                {
                    headingCmd = bearingCmd;
                }
                break;
            case HorizontalControlIF::NONE:
            case HorizontalControlIF::HEADING_RATE:
            case HorizontalControlIF::RUDDER_ANGLE:
            default:
                break;
            }

            if( isnan( headingCmd ) )
            {
                orientationReader_->read( Units::RADIAN, headingCmd ); // TODO: What if we get to this point and orientationReader_->read fails?
            }

            // first project out current heading by 100 meters for comparison
            double newLat( latitude ), newLon( longitude );
            Location::AtBearing( headingCmd, 100.0, newLat, newLon );
            float distance = NavChartDb::GetInstance()->getDistanceFromShore( newLat, newLon );

            // project out 100 meters at +/- pi/16 radians, to see which way to go
            float increment = atan( 1 ) * 0.25;
            bool doneLooking = false;
            for( int i = 1; !doneLooking && i <= 8; ++i )
            {
                for( int j = -1; !doneLooking && j <= 1 ; j += 2 )
                {
                    float newHeading = AuvMath::ModPi( headingCmd + j * i * increment );
                    newLat = latitude;
                    newLon = longitude;
                    Location::AtBearing( newHeading, 100.0, newLat, newLon );
                    float newDistance = NavChartDb::GetInstance()->getDistanceFromShore( newLat, newLon );
                    if( ( distance < minOffshore && newDistance > distance )
                            || ( distance > maxOffshore && newDistance < distance ) )
                    {
                        distance = newDistance;
                        headingCmd = newHeading;
                        if( ( distance >= minOffshore || isnan( minOffshore ) )
                                && ( distance <= maxOffshore || isnan( maxOffshore ) ) )
                        {
                            doneLooking = true;
                        }
                    }
                }
            }

            // Set the heading
            headingCmdWriter_->write( Units::RADIAN, headingCmd );

            // Set the heading mode
            horizontalMode = HorizontalControlIF::HEADING;
            horizontalModeWriter_->write( Units::ENUM, horizontalMode );

            // Return false, because we are not in the envelope
            return false;
        }
        return true;
    }
    return false;
}

/// Uninit function
void OffshoreEnvelope::uninitialize( void )
{
    logger_.syslog( "Uninitialize OffshoreEnvelopeComponent." );
    offshoreDistanceReader_->requestData( false );
    orientationReader_->requestData( false );
}

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