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

#include "HFRCMSurfaceCurrentAtVehicleLocation.h"
#include "HFRCMSurfaceCurrentAtVehicleLocationIF.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/Timestamp.h"

HFRCMSurfaceCurrentAtVehicleLocation::HFRCMSurfaceCurrentAtVehicleLocation( const Module* module )
    : SyncEstimationComponent( HFRCMSurfaceCurrentAtVehicleLocationIF::NAME, module ),
      debug_( false ),
      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
      expansionCoefficients_( logger_, HFRadarCompactModelForecasterIF::MAX_EOFS, HFRadarCompactModelForecasterIF::TOTAL_HOURS ), // initialize the Mtx to hold the expansion coefficients
      expansionCoefficientsNow_( logger_, HFRadarCompactModelForecasterIF::MAX_EOFS, 1 ), // initialize the Mtx to hold the expansion coefficients
      eastVelocity_( nanf( "" ) ),
      northVelocity_( nanf( "" ) ),
      velocityAccuracy_( nanf( "" ) ),
      haveValidForecast_( false ),
      latitudeReader_( newUniversalReader( UniversalURI::LATITUDE ) ),
      longitudeReader_( newUniversalReader( UniversalURI::LONGITUDE ) ),
      velocityAccuracyConfigReader_( newConfigReader( HFRCMSurfaceCurrentAtVehicleLocationIF::VELOCITY_ACCURACY ) )
{
    velocityAccuracyConfigReader_->read( Units::CENTIMETER_PER_SECOND, velocityAccuracy_ );
    eastVelocityWriter_ = newUniversalWriter( UniversalURI::SURFACE_EASTWARD_SEA_WATER_VELOCITY, Units::CENTIMETER_PER_SECOND, velocityAccuracy_ );
    northVelocityWriter_ = newUniversalWriter( UniversalURI::SURFACE_NORTHWARD_SEA_WATER_VELOCITY, Units::CENTIMETER_PER_SECOND, velocityAccuracy_ );
}

HFRCMSurfaceCurrentAtVehicleLocation::~HFRCMSurfaceCurrentAtVehicleLocation()
{}

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

    latitudeReader_->requestData( true );
    longitudeReader_->requestData( true );
}

void HFRCMSurfaceCurrentAtVehicleLocation::run( void )
{
    bool gotECs = lookupExpansionCoefficients();
    bool gotEOFs = lookupEmpiricalOrthogonalFunctions();
    Timestamp calculationCompleteTime = Timestamp::Now();
    if( gotECs && gotEOFs )
    {
        reproject(); // Do the reduced reprojection calculation here.
        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.
        eastVelocityWriter_->setInvalid( false );
        northVelocityWriter_->setInvalid( false );
    }
    else
    {
        velocityAccuracy_ = nan( "" );
        eastVelocity_ = nan( "" );
        northVelocity_ = nan( "" );
        eastVelocityWriter_->setInvalid( true );
        northVelocityWriter_->setInvalid( true );
    }
    eastVelocityWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, eastVelocity_, velocityAccuracy_, calculationCompleteTime );
    northVelocityWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, northVelocity_, velocityAccuracy_, calculationCompleteTime );
}

bool HFRCMSurfaceCurrentAtVehicleLocation::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 HFRCMSurfaceCurrentAtVehicleLocation::lookupEmpiricalOrthogonalFunctions( void )
{
    // TODO: To generalize & subclass an HFRCM component, latitude & longitude would become member parameters and this method would apply for multiple applications.
    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        if( verbosity_ > 0 ) logger_.syslog( "latitude or longitude reader is inactive", Syslog::DEBUG );
        return false;
    }
    float latDeg, lonDeg;
    // read current location
    bool gotLat = latitudeReader_->read( Units::DEGREE, latDeg );
    bool gotLon = longitudeReader_->read( Units::DEGREE, lonDeg );
    if( !gotLat || !gotLon )
    {
        if( verbosity_ > 0 ) logger_.syslog( "could not read latitude or longitude from slate", Syslog::DEBUG );
        return false;
    }
    if( verbosity_ > 0 ) logger_.syslog( "requesting EOFs at (" + Str( latDeg ) + ", " + Str( lonDeg ) + ")", Syslog::INFO );

    // 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_,
                       latDeg, lonDeg );
    return gotEOFs;
}

void HFRCMSurfaceCurrentAtVehicleLocation::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_;
}