/** \file
 *
 *  Contains the CurrentEstimator class implementation.
 *
 *  Copyright (c) 2013 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
*
* This behavior calculates water current in two scenarios: 1.) Bottom lock and water speed are available 2.) Bottom lock is available and commanded speed is 0.
*
 */

#include "CurrentEstimator.h"
#include "CurrentEstimatorIF.h"
#include "data/Slate.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/BlobReader.h"
#include "data/BlobWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "derivationModule/SpeedCalculatorIF.h"

CurrentEstimator::CurrentEstimator( const Str& prefix, const Module* module )
    : Behavior( prefix, module, true, true ),
      verbosity_( -1 ),
      speed_( nan( "" ) ),
      currentSpeedVF_( nan( "" ) ),
      currentSpeedNF_( nan( "" ) ),
      currentDirectionVF_( nan( "" ) ),
      currentDirectionNF_( nan( "" ) ),
      dataStartTime_( Timestamp::NOT_SET_TIME )
{
    vehicleOrientationReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_ORIENTATION_MATRIX );
    velocityBottomReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_VELOCITY_WRT_GROUND );
    velocityWaterReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_VELOCITY_WRT_SEA_WATER );
    speedCalculatorReader_ = newDataReaderFromUniversal( SpeedCalculatorIF::NAME, UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER );

    // Outputs to the slate
    currentSpeedVehicleWriter_ = newDataWriter( CurrentEstimatorIF::CURRENT_SPEED_VF );
    currentSpeedEarthWriter_ = newDataWriter( CurrentEstimatorIF::CURRENT_SPEED_NF );
    currentDirectionVehicleWriter_ = newDataWriter( CurrentEstimatorIF::CURRENT_DIRECTION_VF );
    currentDirectionEarthWriter_ = newDataWriter( CurrentEstimatorIF::CURRENT_DIRECTION_NF );

    vDirectionWriter_ = newBlobWriter( CurrentEstimatorIF::DIRECTION_VF );
    nDirectionWriter_ = newBlobWriter( CurrentEstimatorIF::DIRECTION_NF );

    rotationFromVehicleToNavigationFrame_ = Matrix3x3( 1, 0, 0, 0, 1, 0, 0, 0, 1 );

    velocityRelativeToWaterInVehicleFrame_ = Point3D( 0.0 );
    velocityCurrentRelativeToGroundInVehicleFrame_ = Point3D( 0.0 );
    velocityCurrentRelativeToGroundInNavigationFrame_ = Point3D( 0.0 );
    velocityRelativeToGroundInVehicleFrame_ = Point3D( 0.0 );

}




CurrentEstimator::~CurrentEstimator()
{}


void CurrentEstimator::initialize( void )
{
    logger_.syslog( "Initializing CurrentEstimator.", Syslog::INFO );
    requestVehiclePose( true );
}


void CurrentEstimator::uninitialize( void )
{
    if( verbosity_ > 0 ) logger_.syslog( "Uninitializing CurrentEstimator.", Syslog::DEBUG );
    requestVehiclePose( false );
}


void CurrentEstimator::requestVehiclePose( bool req )
{
    vehicleOrientationReader_->requestData( req );
}


void CurrentEstimator::readSettings( void )
{
// Anticipating some settings someday
}


void CurrentEstimator::run( void )
{

    // Read in mission settings
//    readSettings();

    if( estimateCurrent() )
    {
        publishData();
    }
}


bool CurrentEstimator::estimateCurrent( void )
{
    bool retVal = false;

    // Since this current estimate is based on using a DVL, don't bother doing anything if there's no bottom velocity
    if( velocityBottomReader_->read1DClass( Units::METER_PER_SECOND, velocityRelativeToGroundInVehicleFrame_ ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "velocityRelativeToGroundInVehicleFrame_: " + Str( velocityRelativeToGroundInVehicleFrame_.toString() ), Syslog::INFO );

        // So we have bottom velocity, let's check for water.
        if( velocityWaterReader_->read1DClass( Units::METER_PER_SECOND, velocityRelativeToWaterInVehicleFrame_ ) )
        {
            if( verbosity_ > 0 ) logger_.syslog( "velocityRelativeToWaterInVehicleFrame_: " + Str( velocityRelativeToWaterInVehicleFrame_.toString() ), Syslog::INFO );
        }

        // And finally if we don't know how fast we're moving wrt water, just calculate if we're drifting.
        else if( ( speedCalculatorReader_->read( Units::METER_PER_SECOND, speed_ ) ) && ( speed_ == 0 ) )
        {
            // But here we have both, so let's assume the course of the vehicle is along its longitudinal axis.
            velocityRelativeToWaterInVehicleFrame_.setU( speed_ );
            velocityRelativeToWaterInVehicleFrame_.setV( 0.0 );
            velocityRelativeToWaterInVehicleFrame_.setW( 0.0 );
            if( verbosity_ > 0 ) logger_.syslog( "Drift mode velocityRelativeToWaterInVehicleFrame: " + Str( velocityRelativeToWaterInVehicleFrame_.toString() ), Syslog::INFO );
        }

        // We're not drifting and no water speed is present so no need to calculate current.
        else
        {
            return retVal;
        }

        // Calculate current magnitude and angles with respect to the vehicle
        velocityCurrentRelativeToGroundInVehicleFrame_ = velocityRelativeToWaterInVehicleFrame_ - velocityRelativeToGroundInVehicleFrame_;
        currentSpeedVF_ =  velocityCurrentRelativeToGroundInVehicleFrame_.getMagnitude();
        if( verbosity_ > 0 ) logger_.syslog( "velocityCurrentRelativeToGroundInVehicleFrame_: " + Str( velocityCurrentRelativeToGroundInVehicleFrame_.toString() ), Syslog::INFO );
        if( verbosity_ > 0 ) logger_.syslog( "velocityCurrent Mag: " + Str( velocityCurrentRelativeToGroundInVehicleFrame_.getMagnitude() ), Syslog::INFO );

        float theta, phi = 0;
        AuvMath::UnitVectorToAzimuthAndElevation( velocityCurrentRelativeToGroundInVehicleFrame_, theta, phi );
        currentDirectionVF_ = R2D( AuvMath::NormalizeAngle( theta - M_PI ) );  // flipping direction. Current is direction to, wind is direction from
        if( verbosity_ > 0 ) logger_.syslog( "velocityCurrent Angles Vehicle Frame: " + Str( R2D( theta ) ) + "," + Str( R2D( phi ) ), Syslog::INFO );

        // Now convert to Earth frame of reference
        if( vehicleOrientationReader_->read2DClass( Units::NONE, rotationFromVehicleToNavigationFrame_ ) )
        {
            velocityCurrentRelativeToGroundInNavigationFrame_ = 0; // zero this vector before using Point3D::addProduct on the next line
            velocityCurrentRelativeToGroundInNavigationFrame_.addProduct( rotationFromVehicleToNavigationFrame_, velocityCurrentRelativeToGroundInVehicleFrame_ );
            currentSpeedNF_ = velocityCurrentRelativeToGroundInNavigationFrame_.getMagnitude();
            if( verbosity_ > 0 ) logger_.syslog( "velocityCurrent Mag Navigation Frame: " + Str( velocityCurrentRelativeToGroundInNavigationFrame_.getMagnitude() ), Syslog::INFO );

            AuvMath::UnitVectorToAzimuthAndElevation( velocityCurrentRelativeToGroundInNavigationFrame_, theta, phi );
            currentDirectionNF_ = R2D( AuvMath::NormalizeAngle( theta - M_PI ) ); // flipping direction. Current is direction to, wind is direction from
            if( verbosity_ > 0 ) logger_.syslog( "velocityCurrent Angles Navigation Frame: " + Str( R2D( theta ) ) + "," + Str( R2D( phi ) ), Syslog::INFO );

            retVal = true; // Currents have been estimated
        }

    }
    return retVal;
}


void CurrentEstimator::publishData()
{
    dataStartTime_ = Timestamp::Now();
    currentSpeedVehicleWriter_->write( Units::METER_PER_SECOND, currentSpeedVF_, dataStartTime_ );
    currentSpeedEarthWriter_->write( Units::METER_PER_SECOND, currentSpeedNF_, dataStartTime_ );
    currentDirectionVehicleWriter_->write( Units::DEGREE, currentDirectionVF_, dataStartTime_ );
    currentDirectionEarthWriter_->write( Units::DEGREE, currentDirectionNF_, dataStartTime_ );
}


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