/** \file
 *
 *  Contains the HFRCMVirtualSurfaceDrifter class implementation.
 *
 *  Copyright (c) 2013, 2014 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "HFRCMVirtualSurfaceDrifter.h"
#include "HFRCMVirtualSurfaceDrifterIF.h"

#include "HFRadarCompactModelForecaster.h" // for MAX_EOFS, etc (consider moving thoseinto the interface file instead...)
#include "HFRadarCompactModelForecasterIF.h"

#include "HFRCMTimeInterpolator.h" // for public method lookupExpansionCoefficients
#include "HFRCMSpaceInterpolator.h" // for public method lookupEmpiricalOrthogonalFunctions

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "data/BlobReader.h"
#include "data/BlobValue.h"
#include "data/Mtx.h"
#include "utils/AuvMath.h"
#include "utils/Timestamp.h"

HFRCMVirtualSurfaceDrifter* HFRCMVirtualSurfaceDrifter::Instance_( NULL ); // set up the singleton so that we can support static accessor functions

HFRCMVirtualSurfaceDrifter::HFRCMVirtualSurfaceDrifter( const Module* module )
    : SyncEstimationComponent( HFRCMVirtualSurfaceDrifterIF::NAME, module ),
      verbosity_( 0 ),
      eastEmpiricalOrthogonalFunctionsHere_( logger_, 1, HFRadarCompactModelForecasterIF::MAX_EOFS ), // initialize the Mtx to hold the East EOFs
      northEmpiricalOrthogonalFunctionsHere_( logger_, 1, HFRadarCompactModelForecasterIF::MAX_EOFS ), // initialize the Mtx to hold the North EOFs
      expansionCoefficientsNow_( logger_, HFRadarCompactModelForecasterIF::MAX_EOFS, 1 ), // initialize the Mtx to hold the expansion coefficients
      calculationTime_( Timestamp::Now() ),
      elapsedTime_( 0.0 ),
      eastVelocity_( nan( "" ) ),
      northVelocity_( nan( "" ) ),
      velocityAccuracy_( 0.0 ),
      latitude_( nan( "" ) ),
      longitude_( nan( "" ) ),
      velocityAccuracyConfigReader_( newConfigReader( HFRCMVirtualSurfaceDrifterIF::VELOCITY_ACCURACY ) )
{
    eastVelocityWriter_ = newDataWriter( HFRCMVirtualSurfaceDrifterIF::EASTWARD_VELOCITY );//, Units::CENTIMETER_PER_SECOND );
    northVelocityWriter_ = newDataWriter( HFRCMVirtualSurfaceDrifterIF::NORTHWARD_VELOCITY );//, Units::CENTIMETER_PER_SECOND );
    latitudeWriter_ = newDataWriter( HFRCMVirtualSurfaceDrifterIF::LATITUDE );//, Units::RADIAN );
    longitudeWriter_ = newDataWriter( HFRCMVirtualSurfaceDrifterIF::LONGITUDE );//, Units::RADIAN );
    initialLatitudeWriter_ = newDataWriter( HFRCMVirtualSurfaceDrifterIF::INITIAL_LATITUDE );//, Units::RADIAN );
    initialLongitudeWriter_ = newDataWriter( HFRCMVirtualSurfaceDrifterIF::INITIAL_LONGITUDE );//, Units::RADIAN );

    Instance_ = this;

//    HFRCMTimeInterpolator* timeInterpolator = HFRCMTimeInterpolator::Instance();
//    HFRCMSpaceInterpolator* spaceInterpolator = HFRCMSpaceInterpolator::Instance();
}

HFRCMVirtualSurfaceDrifter::~HFRCMVirtualSurfaceDrifter()
{
    Instance_ = NULL;
}

void HFRCMVirtualSurfaceDrifter::initialize( void )
{
    logger_.syslog( "Initializing HFRCMVirtualSurfaceDrifter component." );
}

bool HFRCMVirtualSurfaceDrifter::Reinitialize( double latitude0rad, double longitude0rad )
{
    if( NULL == Instance_ )
    {
        return false;
    }
    return Instance_->setLocation( latitude0rad, longitude0rad );
}

bool HFRCMVirtualSurfaceDrifter::setLocation( double latitude0rad, double longitude0rad )
{
    Timestamp initializeTime = Timestamp::Now();
    logger_.syslog( "Setting HFRCMVirtualSurfaceDrifter position: " + Str( R2D( latitude0rad ) ) + ", " + Str( R2D( longitude0rad ) ), Syslog::INFO );
    latitude_ = latitude0rad;
    longitude_ = longitude0rad;
    initialLatitudeWriter_->write( Units::RADIAN, latitude0rad, initializeTime );
    initialLongitudeWriter_->write( Units::RADIAN, longitude0rad, initializeTime );
    return true;
}

void HFRCMVirtualSurfaceDrifter::run( void )
{
    calculationTime_ = Timestamp::Now();
    if( verbosity_ > 0 )
    {
        logger_.syslog( "requesting ECs at " + calculationTime_.toString(), Syslog::INFO );
        logger_.syslog( "requesting EOFs at (" + Str( R2D( latitude_ ) ) + ", " + Str( R2D( longitude_ ) ) + ")", Syslog::INFO );
    }
    bool gotECs = lookupExpansionCoefficients();
    bool gotEOFs = lookupEmpiricalOrthogonalFunctions();
    if( gotECs && gotEOFs )
    {
        reproject(); // Do the reduced reprojection calculation here.
        advect();

        eastVelocityWriter_->setInvalid( false );
        northVelocityWriter_->setInvalid( false );
        latitudeWriter_->setInvalid( false );
        longitudeWriter_->setInvalid( false );
    }
    else
    {
        eastVelocity_ = nan( "" );
        northVelocity_ = nan( "" );
        latitude_ = nan( "" );
        longitude_ = nan( "" );

        eastVelocityWriter_->setInvalid( true );
        northVelocityWriter_->setInvalid( true );
        latitudeWriter_->setInvalid( true );
        longitudeWriter_->setInvalid( true );
    }
    writeData();
}

bool HFRCMVirtualSurfaceDrifter::lookupExpansionCoefficients( void )
{
    // look up the EC values at the current instant
    HFRCMTimeInterpolator* interpolator = HFRCMTimeInterpolator::Instance();
    // TODO: test for existing instance...
    bool gotECs =  interpolator->lookupExpansionCoefficients( Timestamp::Now(), expansionCoefficientsNow_ );
    return gotECs;
}

bool HFRCMVirtualSurfaceDrifter::lookupEmpiricalOrthogonalFunctions( void )
{
    if( isnan( latitude_ ) || isnan( longitude_ ) )
    {
        return false; // Don't bother trying to look up the EOFs.
    }
    // look up the EOF values at current location
    HFRCMSpaceInterpolator* interpolator = HFRCMSpaceInterpolator::Instance();
    // TODO: test for existing instance...
    bool gotEOFs = interpolator->lookupEmpiricalOrthogonalFunctions(
                       eastEmpiricalOrthogonalFunctionsHere_, northEmpiricalOrthogonalFunctionsHere_,
                       eastEnsembleMeanHere_,  northEnsembleMeanHere_,
                       eastScalingFactor_, northScalingFactor_,
                       R2D( latitude_ ), R2D( longitude_ ) );
    return gotEOFs;
}

void HFRCMVirtualSurfaceDrifter::reproject( void )
{
    Mtx east = eastEmpiricalOrthogonalFunctionsHere_ * expansionCoefficientsNow_; // 1xMAX_EOFS x MAX_EOFSx1 = 1x1
    Mtx north = northEmpiricalOrthogonalFunctionsHere_ * expansionCoefficientsNow_; // 1xMAX_EOFS x MAX_EOFSx1 = 1x1
    // TODO: Would be nice if the matrix products above could just return a float instead of a 1x1 Mtx...
    eastVelocity_ = ( east( 0, 0 ) + eastEnsembleMeanHere_ ) * eastScalingFactor_;
    northVelocity_ = ( north( 0, 0 ) + northEnsembleMeanHere_ ) * northScalingFactor_;
}

void HFRCMVirtualSurfaceDrifter::advect( void )
{
    elapsedTime_ = AuvMath::NanZero( dt_.asDouble() ); // dt_.asDouble() is nan for the first cycle
    easting_ = 0.01 * eastVelocity_ * elapsedTime_; // explicit conversion cm/s --> m/s
    northing_ = 0.01 * northVelocity_ * elapsedTime_; // explicit conversion cm/s --> m/s
    course_ = atan2( easting_, northing_ ); // TODO: Do we actually use course anywhere? should we write it to the slate?
    horizontalDisplacement_ = sqrt( pow( northing_, 2 ) + pow( easting_, 2 ) );
    Location::AtBearing( course_, horizontalDisplacement_, latitude_, longitude_ );

    velocityAccuracyConfigReader_->read( Units::CENTIMETER_PER_SECOND, velocityAccuracy_ ); // If you read it each cycle, you should be able to change it on the fly using ConfigSet.
    // TODO: Make velocityAccuracy_ variable, based on estimate from HFRadarCompactModelForecaster component using time since last historical EC. NOTE: Consider interactions with HFRadarModelCalc when adjusting this accuracy.
    locationAccuracy_ += velocityAccuracy_ * elapsedTime_ * Location::EARTH_RADIUS_INVERTED;
}

void HFRCMVirtualSurfaceDrifter::writeData( void )
{

    eastVelocityWriter_->write( Units::CENTIMETER_PER_SECOND, eastVelocity_, calculationTime_ );
    northVelocityWriter_->write( Units::CENTIMETER_PER_SECOND, northVelocity_, calculationTime_ );
    latitudeWriter_->write( Units::RADIAN, latitude_, calculationTime_ );
    longitudeWriter_->write( Units::RADIAN, longitude_, calculationTime_ );
//    eastVelocityWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, eastVelocity_, velocityAccuracy_, calculationCompleteTime_ );
//    northVelocityWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, northVelocity_, velocityAccuracy_, calculationCompleteTime_ );
//    latitudeWriter_->writeWithAccuracy( Units::RADIAN, latitude_, locationAccuracy_, calculationCompleteTime_ );
//    longitudeWriter_->writeWithAccuracy( Units::RADIAN, longitude_, locationAccuracy_, calculationCompleteTime_ );
    // TODO: also write course and horizontalDisplacement out to the slate
}