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

#include "WaterDepthEnvelope.h"
#include "WaterDepthEnvelopeIF.h"

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

WaterDepthEnvelope::WaterDepthEnvelope( const Str& prefix, const Module* module )
    : Behavior( prefix + WaterDepthEnvelopeIF::NAME, module, true, true )
{

    logger_.syslog( "Construct WaterDepthEnvelope." );

    // Slate input setting variables
    minWaterDepthSettingReader_ = newSettingReader( WaterDepthEnvelopeIF::MIN_WATER_DEPTH_SETTING );
    maxWaterDepthSettingReader_ = newSettingReader( WaterDepthEnvelopeIF::MAX_WATER_DEPTH_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 );
    headingCmdReader_ = newDataReader( HorizontalControlIF::HEADING_CMD );
    speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD );

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

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

}

WaterDepthEnvelope::~WaterDepthEnvelope()
{}

/// Initialize function
void WaterDepthEnvelope::initialize( void )
{
    logger_.syslog( "Initialize WaterDepthEnvelopeComponent." );

    seaFloorDepthReader_->requestData( true );
    orientationReader_->requestData( true );
}

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

    // Let's read in all the relevant readerSettings
    bool gotMin = minWaterDepthSettingReader_->read( Units::METER, minWaterDepth );
    bool gotMax = maxWaterDepthSettingReader_->read( Units::METER, maxWaterDepth );
    if( !gotMin && !gotMax )
    {
        logger_.syslog( "Neither min nor max setting specified.", Syslog::ERROR );
    }

    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 );

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

}

/// Perform the satisfied: return true if envelope "satisfied"
bool WaterDepthEnvelope::calcSatisfied( const float minWaterDepth, const float maxWaterDepth,
                                        const float seaFloorDepth )
{
    bool satisfied( false );
    // Now we get to work, iff location is valid!...
    satisfied = isnan( seaFloorDepth ) ||
                ( ( isnan( minWaterDepth ) || seaFloorDepth >= minWaterDepth ) &&
                  ( isnan( maxWaterDepth ) || seaFloorDepth <= maxWaterDepth ) );
    return satisfied;
}

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

/// Just do the satisfied: return true if envelope "satisfied"
bool WaterDepthEnvelope::isSatisfied()
{
    float minWaterDepth( nanf( "" ) );
    float maxWaterDepth( nanf( "" ) );
    HorizontalControlIF::HorizontalMode horizontalMode( HorizontalControlIF::NONE );
    float speed( nanf( "" ) );
    float latitude( nanf( "" ) ), longitude( nanf( "" ) ), seaFloorDepth( nanf( "" ) );

    if( readParams( minWaterDepth, maxWaterDepth, horizontalMode, speed, latitude, longitude, seaFloorDepth ) )
    {
        return calcSatisfied( minWaterDepth, maxWaterDepth, seaFloorDepth );
    }
    return false;
}

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

    float minWaterDepth( nanf( "" ) );
    float maxWaterDepth( nanf( "" ) );
    HorizontalControlIF::HorizontalMode horizontalMode( HorizontalControlIF::NONE );
    float speedCmd( nanf( "" ) );
    float latitude( nanf( "" ) ), longitude( nanf( "" ) ), seaFloorDepth( nanf( "" ) );

    if( readParams( minWaterDepth, maxWaterDepth, horizontalMode, speedCmd, latitude, longitude, seaFloorDepth ) )
    {
        if( latitude != latitude || longitude != longitude )
        {
            return false;
        }

        if( !calcSatisfied( minWaterDepth, maxWaterDepth, seaFloorDepth ) )
        {

            // Now figure out our current heading
            float headingCmd( 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 );
                headingCmd = Location::GetBearing( latitude, longitude, latitudeCmd, longitudeCmd );
                break;
            case HorizontalControlIF::NONE:
            case HorizontalControlIF::HEADING_RATE:
            case HorizontalControlIF::RUDDER_ANGLE:
            default:
                orientationReader_->read( Units::RADIAN, headingCmd );
                break;
            }

            // first project out current heading by 0.0 meters for comparison
            double newLatitude( latitude );
            double newLongitude( longitude );
            Location::AtBearing( headingCmd, 0.0, newLatitude, newLongitude );
            float bathymetry = NavChartDb::GetInstance()->getSeaFloorDepth( newLatitude, newLongitude );
            float minBath = nanf( "" );
            float maxBath = nanf( "" );

            // project out 200.0 meters at +/- pi/16 radians, to see which way to go
            float increment = atan( 1 ) * 0.25;
            bool headingChanged = false;
            for( int i = 1; i <= 8; ++i )
            {
                for( int j = -1; j <= 1 ; j += 2 )
                {
                    bool isMax = false;
                    bool isMin = false;
                    float newHeading = AuvMath::ModPi( headingCmd + j * i * increment );
                    newLatitude = latitude;
                    newLongitude = longitude;
                    Location::AtBearing( newHeading, 200.0, newLatitude, newLongitude );
                    float newBathymetry = NavChartDb::GetInstance()->getSeaFloorDepth( newLatitude, newLongitude );
                    //printf( "At newHeading=%f, minWaterDepth=%g, newBath=%g\n", R2D( newHeading ), minWaterDepth, newBathymetry );
                    if( isnan( newBathymetry ) ) continue;
                    if( isnan( minBath ) || newBathymetry < minBath )
                    {
                        isMin = true;
                        minBath = newBathymetry;
                    }
                    if( isnan( maxBath ) || newBathymetry > maxBath )
                    {
                        isMax = true;
                        maxBath = newBathymetry;
                    }
                    if( ( !isnan( minWaterDepth ) && bathymetry < minWaterDepth && isMax )
                            || ( !isnan( maxWaterDepth ) && bathymetry > maxWaterDepth && isMin ) )
                    {
                        headingCmd = newHeading;
                        headingChanged = true;
                    } // else printf("!headingChanged because minWaterDepth=%g, bathymetry=%g, newBathymetry=%g, maxBath=%g, maxWaterDepth=%g\n", minWaterDepth, bathymetry, newBathymetry, maxBath, maxWaterDepth);
                }
            }

            // Avoid moving if no real slope
            if( !headingChanged || isnan( minBath ) || ( maxBath - minBath ) / maxBath < 0.01 )
            {
                return false;
            }

            // Set the heading
            // printf( "***** Set headingCmd=%f\n", R2D( headingCmd ) );
            headingCmdWriter_->write( Units::RADIAN, headingCmd );

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

            // Need to move! -- so make sure we have some speed.
            if( isnan( speedCmd ) || speedCmd < SpeedControlIF::SPEED_FAST )
            {
                speedCmd = SpeedControlIF::SPEED_FAST;
            }
            speedCmdWriter_->write( Units::METER_PER_SECOND, speedCmd );

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

/// Uninit function
void WaterDepthEnvelope::uninitialize( void )
{
    logger_.syslog( "Uninitialize WaterDepthEnvelopeComponent." );
    seaFloorDepthReader_->requestData( false );
    orientationReader_->requestData( false );
}

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