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

#include "Lane.h"
#include "LaneIF.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"

Lane::Lane( const Str& prefix, const Module* module )
    : Behavior( prefix + LaneIF::NAME, module, true, true ),
      bearingSetting_( nan( "" ) ),
      distanceSetting_( nan( "" ) ),
      latitudeSetting_( nan( "" ) ),
      longitudeSetting_( nan( "" ) ),
      widthSetting_( nan( "" ) ),
      offsetSetting_( nan( "" ) ),
      latitude_( nan( "" ) ),
      longitude_( nan( "" ) ),
      perpendicularSlope_( nan( "" ) ),
      startOverLine_( false ),
      initialized_( false )
{

    logger_.syslog( "Construct Lane." );

    // Slate input setting variables
    bearingSettingReader_ = newSettingReader( LaneIF::BEARING_SETTING );
    distanceSettingReader_ = newSettingReader( LaneIF::DISTANCE_SETTING );
    widthSettingReader_ = newSettingReader( LaneIF::WIDTH_SETTING );
    offsetSettingReader_ = newSettingReader( LaneIF::OFFSET_SETTING );

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

    horizontalModeReader_ = newDataReader( HorizontalControlIF::HORIZONTAL_MODE );
    headingCmdReader_ = newDataReader( HorizontalControlIF::HEADING_CMD );
    bearingCmdReader_ = newDataReader( HorizontalControlIF::BEARING_CMD );

    // Slate output variables
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    headingCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD );
}

Lane::~Lane()
{}

void Lane::readSettings( void )
{
    bearingSettingReader_->read( Units::RADIAN, bearingSetting_ );
    distanceSettingReader_->read( Units::METER, distanceSetting_ );
    widthSettingReader_->read( Units::METER, widthSetting_ );

    if( !offsetSettingReader_->read( Units::METER, offsetSetting_ )
            || isnan( offsetSetting_ ) )
    {
        offsetSetting_ = 0;
    }
}

/// Initialize function
void Lane::initialize( void )
{
    logger_.syslog( "Initialize LaneComponent." );

    bearingSetting_ = nan( "" );
    distanceSetting_ = nan( "" );
    latitudeSetting_ = nan( "" );
    longitudeSetting_ = nan( "" );
    widthSetting_ = nan( "" );
    offsetSetting_ = nan( "" );

    if( !bearingSettingReader_->isActive() )
    {
        logger_.syslog( "Missing bearing setting.", Syslog::ERROR );
        initialized_ = false;
        return;
    }

    if( !distanceSettingReader_->isActive() )
    {
        logger_.syslog( "Missing distance setting.", Syslog::ERROR );
        initialized_ = false;
        return;
    }

    if( !widthSettingReader_->isActive() )
    {
        logger_.syslog( "Missing width setting.", Syslog::ERROR );
        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 ) )
    {
        initialized_ = false;
        return;
    }
    latitudeSetting_ = startLatitude;
    longitudeSetting_ = startLongitude;

    // Read settings
    readSettings();

    Location::AtBearing( bearingSetting_, distanceSetting_, latitudeSetting_, longitudeSetting_ );

    /// Slope of the line perpendicular between now and the lane
    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_;
    }

    headingReader_->requestData( true );

    initialized_ = true;

}

bool Lane::isSatisfied()
{
    return readParams() && calcSatisfied();
}

void Lane::run()
{
    if( readParams() )
    {
        doRun();
    }
}

bool Lane::runIfUnsatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        satisfied = isSatisfied();
        if( satisfied )
        {
            logger_.syslog( "Reached Far Lane Edge: " + Str( R2D( latitudeSetting_ ) ) + "," + Str( R2D( longitudeSetting_ ) ), Syslog::INFO );
        }
        doRun();
    }
    return satisfied;
}

bool Lane::readParams()
{
    if( !initialized_ )
    {
        initialize();
    }
    else
    {
        readSettings();
    }

    if( !initialized_ )
    {
        return false;
    }

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

    latitudeReader_->read( Units::RADIAN, latitude_ );
    longitudeReader_->read( Units::RADIAN, longitude_ );

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

    return true;
}

bool Lane::calcSatisfied()
{
    bool isOverLine;
    if( !isnan( perpendicularSlope_ ) )
    {
        isOverLine = latitude_ > latitudeSetting_
                     + ( longitude_ - longitudeSetting_ ) * perpendicularSlope_;
    }
    else
    {
        isOverLine = longitude_ > longitudeSetting_;
    }

    return isOverLine != startOverLine_;
}

///
void Lane::doRun()
{

    // Compute differential northings and eastings:
    float dn = ( latitudeSetting_ - latitude_ )   * Location::EARTH_RADIUS;
    float de = ( longitudeSetting_ - longitude_ ) * Location::EARTH_RADIUS
               * cos( latitude_ );
    //
    // Find the cross-track error positve values are to the left of the bearing,
    // negative values are to the right of the bearing:
    float xte   = -sin( bearingSetting_ ) * dn + cos( bearingSetting_ ) * de;

    int turn( 0 );
    if( -xte > widthSetting_ / 2 + offsetSetting_ )
    {
        // Need to bear left
        turn = -1;
    }
    else if( xte > widthSetting_ / 2 - offsetSetting_ )
    {
        // Need to bear right
        turn = 1;
    }

    //printf( "xte=%f, widthSetting_=%f, offsetSetting_=%f, turn=%d\n", xte, widthSetting_, offsetSetting_, turn );

    if( turn != 0 )
    {
        int headingMode;
        float heading;
        if( horizontalModeReader_->isActive()
                && horizontalModeReader_->wasTouchedSinceLastRun( this )
                && horizontalModeReader_->read( Units::ENUM, headingMode )
                && ( headingMode == HorizontalControlIF::HEADING
                     || headingMode == HorizontalControlIF::WAYPOINT ) )
        {
            if( headingMode == HorizontalControlIF::HEADING )
            {
                headingCmdReader_->read( Units::RADIAN, heading );
            }
            else if( headingMode == HorizontalControlIF::WAYPOINT )
            {
                bearingCmdReader_->read( Units::RADIAN, heading );
            }
        }
        else
        {
            headingReader_->read( Units::RADIAN, heading );
        }

        if( turn < 0 && AuvMath::ModPi( heading - bearingSetting_ ) > -M_PI_4 )
        {
            // turn left
            heading = AuvMath::ModPi( bearingSetting_ - M_PI_4 );
        }
        else if( turn > 0 && AuvMath::ModPi( heading - bearingSetting_ ) < M_PI_4 )
        {
            // turn right
            heading = AuvMath::ModPi( bearingSetting_ + M_PI_4 );
        }

        horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
        headingCmdWriter_->write( Units::RADIAN, heading );
    }

}
/// Uninit function
void Lane::uninitialize( void )
{
    logger_.syslog( "Uninitialize LaneComponent." );
    headingReader_->requestData( false );
    initialized_ = false;
}

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