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

#include "HFRCMReconstructedInterpolator.h"
#include "HFRCMReconstructedInterpolatorIF.h"

#include "HFRadarCompactModelForecasterIF.h"

#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"

HFRCMReconstructedInterpolator* HFRCMReconstructedInterpolator::Instance_( NULL );

HFRCMReconstructedInterpolator::HFRCMReconstructedInterpolator( const Module* module )
    : SyncEstimationComponent( HFRCMReconstructedInterpolatorIF::NAME, module ),
      debug_( false ),
      verbosity_( 0 ),
      eofs_( "Resources/HFRadarModel/1.U.mtx", logger_, 0, HFRadarCompactModelForecasterIF::MAX_EOFS ),
      ensMean_( "Resources/HFRadarModel/1.ens_mean.mtx", logger_ ),
      gridIdxRev_( "Resources/HFRadarModel/gr.hgrid.idx_x2X.mtx", logger_ ),
      gridLon_( "Resources/HFRadarModel/gr.hgrid.X.mtx", logger_ ),
      gridLat_( "Resources/HFRadarModel/gr.hgrid.Y.mtx", logger_ ),
      scalingFactors_( "Resources/HFRadarModel/stateInfo.std.all.mtx", logger_ ),
      gridIdx_( logger_, gridLat_.getM(), gridLon_.getN() ),
      numLocations_( ensMean_.getM() / 2 ),
      numModes_( HFRadarCompactModelForecasterIF::MAX_EOFS ),
      forecastStart_( Timestamp::NOT_SET_TIME ),
      forecastEnd_( Timestamp::NOT_SET_TIME ),
      expansionCoefficients_( logger_, HFRadarCompactModelForecasterIF::MAX_EOFS, HFRadarCompactModelForecasterIF::TOTAL_HOURS ), // initialize the Mtx to hold the expansion coefficients
      currentField_( logger_, eofs_.getM(), HFRadarCompactModelForecasterIF::TOTAL_HOURS ), // initialize the Mtx to hold the reconstructed current field
      eastVelocity_( nan( "" ) ),
      northVelocity_( nan( "" ) ),
      velocityAccuracy_( nan( "" ) ),
      haveValidForecast_( false ),
      latitudeReader_( newUniversalReader( UniversalURI::LATITUDE ) ),
      longitudeReader_( newUniversalReader( UniversalURI::LONGITUDE ) ),
      expansionCoefficientsReader_( newBlobReader( HFRadarCompactModelForecasterIF::FORECAST_EXPANSION_COEFFICIENTS ) ),
      forecastTimesReader_( newBlobReader( HFRadarCompactModelForecasterIF::FORECAST_TIMES ) ),
      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_ ) ),
      verbosityLevelConfigReader_( newConfigReader( HFRCMReconstructedInterpolatorIF::VERBOSITY ) ),
      velocityAccuracyConfigReader_( newConfigReader( HFRCMReconstructedInterpolatorIF::VELOCITY_ACCURACY ) )
{
    Instance_ = this;
}

HFRCMReconstructedInterpolator::~HFRCMReconstructedInterpolator()
{}

void HFRCMReconstructedInterpolator::initialize( void )
{
    velocityAccuracyConfigReader_->read( Units::CENTIMETER_PER_SECOND, velocityAccuracy_ ); // read this once during initialization (then again each time you are about to publish the surface current at the vehicle location)
    logger_.syslog( "Initializing HFRCMReconstructedInterpolator component with velocityAccuracy_ = " + Str( 0.01 * velocityAccuracy_ ) + " m/s.", Syslog::INFO );

    latitudeReader_->requestData( true );
    longitudeReader_->requestData( true );
    expansionCoefficientsReader_->requestData( true );
    forecastTimesReader_->requestData( true );

    for( int i = 0; i < gridIdxRev_.getM(); ++i )
    {
        int idxRev = ( int )gridIdxRev_( i, 0 );
        int n = idxRev / gridIdx_.getM();
        int m = idxRev - n * gridIdx_.getM();
        gridIdx_[m][n] = i;
    }
    if( verbosity_ > 0 )
    {
        logger_.syslog( "gridIdxRev_: " + Str( gridIdxRev_.getM() ) + " by " + Str( gridIdxRev_.getN() ) + ", [" + Str( gridIdxRev_( 0, 0 ) ) + ", " + Str( gridIdxRev_( -1, -1 ) ) + "]", Syslog::INFO );
        logger_.syslog( "gridIdx_: " + Str( gridIdx_.getM() ) + " by " + Str( gridIdx_.getN() ) + ", [" + Str( gridIdx_( 0, 0 ) ) + ", " + Str( gridIdx_( -1, -1 ) ) + "]", Syslog::INFO );
        logger_.syslog( "longitude grid: " + Str( gridLon_.getM() ) + " by " + Str( gridLon_.getN() ) + ", [" + Str( gridLon_( 0, 0 ) ) + ", " + Str( gridLon_( 0, -1 ) ) + "]", Syslog::INFO );
        logger_.syslog( "latitude grid: " + Str( gridLat_.getM() ) + " by " + Str( gridLat_.getN() ) + ", [" + Str( gridLat_( 0, 0 ) ) + ", " + Str( gridLat_( -1, 0 ) ) + "]", Syslog::INFO );
    }
}

void HFRCMReconstructedInterpolator::run( void )
{
    if( expansionCoefficientsReader_->isActive() && expansionCoefficientsReader_->wasTouchedSinceLastRun( this ) )
    {
        updateExpansionCoefficients();
        reproject();
    }

    if( haveValidForecast_ )
    {
        if( true ) // TODO: add another config setting; If configured to do so, periodically publish the surface current at the vehicle's current location to the slate.
        {
            publishSurfaceCurrentAtVehicleLocation();
        }
    }
    else
    {
        logger_.syslog( "no valid forecast", Syslog::DEBUG );
    }
}

void HFRCMReconstructedInterpolator::publishSurfaceCurrentAtVehicleLocation( void )
{
    if( verbosity_ > 0 ) logger_.syslog( "calculating velocity at vehicle location", Syslog::INFO );
    float latitude_degrees, longitude_degrees;
    bool gotLat = latitudeReader_->read( Units::DEGREE, latitude_degrees );
    bool gotLon = longitudeReader_->read( Units::DEGREE, longitude_degrees );
    if( gotLat && gotLon )
    {
        Timestamp now = Timestamp::Now();
        bool gotCurrent = lookupSurfaceCurrent( eastVelocity_, northVelocity_, now.asDouble(), latitude_degrees, longitude_degrees );

        if( gotCurrent )
        {
            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_, now );
        northVelocityWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, northVelocity_, velocityAccuracy_, now );
    }
}

bool HFRCMReconstructedInterpolator::lookupSurfaceCurrent( float & eastVelocity, float & northVelocity, const double epoch, const float latitude_degrees, const float longitude_degrees )
{
    if( verbosity_ > 0 ) logger_.syslog( "received request for location (" + Str( latitude_degrees ) + ", " + Str( longitude_degrees ) + ")", Syslog::INFO );
    if( !haveValidForecast_ ) return false;

    int time_index( -1 ), latitude_index( -1 ), longitude_index( -1 );
    if( getPreviousTimeIndex( time_index, epoch ) && getLowerLeftCorner( latitude_index, longitude_index, latitude_degrees, longitude_degrees ) )
    {
        if( verbosity_ > 2 ) logger_.syslog( "time starting: " + Timestamp( forecastEpochSeconds_[time_index] ).toString() + "; currentField_(0," + Str( time_index ) + ") = " + Str( currentField_( 0, time_index ) ), Syslog::INFO );
        if( verbosity_ > 1 ) logger_.syslog( "found previous time at index " + Str( time_index ) + ", and lower left corner at indices: " + Str( latitude_index ) + ", " + Str( longitude_index ), Syslog::INFO );
        eastVelocity_ = interpolateRaveledThreeDimensionalGrid( currentField_, epoch, latitude_degrees, longitude_degrees, time_index, latitude_index, longitude_index, 0 );
        northVelocity_ = interpolateRaveledThreeDimensionalGrid( currentField_, epoch, latitude_degrees, longitude_degrees, time_index, latitude_index, longitude_index, numLocations_ );
        return true;
    }
    else
    {
        return false;
    }
}

float HFRCMReconstructedInterpolator::interpolateRaveledThreeDimensionalGrid( const Mtx z, const float epoch, const float latitude_degrees, const float longitude_degrees, const int time_index, const int latitude_index, const int longitude_index, const int starting_offset )
{
    float local[2][2][2] = { { { 0.0 } } }; // start with everything at zero -- only fill it in if there is a valid value

    // TODO: Confirm the index ordering of this seed makes sense with AuvMath::Interpolate3D
    if( !isnan( gridIdx_[latitude_index][longitude_index] ) )
    {
        local[0][0][0] = z( int( gridIdx_( latitude_index, longitude_index ) ) + starting_offset, time_index );
        local[1][0][0] = z( int( gridIdx_( latitude_index, longitude_index ) ) + starting_offset, time_index + 1 );
    }
    if( !isnan( gridIdx_( latitude_index, longitude_index + 1 ) ) )
    {
        local[0][0][1] = z( int( gridIdx_( latitude_index, longitude_index + 1 ) ) + starting_offset, time_index );
        local[1][0][1] = z( int( gridIdx_( latitude_index, longitude_index + 1 ) ) + starting_offset, time_index + 1 );
    }
    if( !isnan( gridIdx_( latitude_index + 1, longitude_index + 1 ) ) )
    {
        local[0][1][1] = z( int( gridIdx_( latitude_index + 1, longitude_index + 1 ) ) + starting_offset, time_index );
        local[1][1][1] = z( int( gridIdx_( latitude_index + 1, longitude_index + 1 ) ) + starting_offset, time_index + 1 );
    }
    if( !isnan( gridIdx_( latitude_index + 1, longitude_index ) ) )
    {
        local[0][1][0] = z( int( gridIdx_( latitude_index + 1, longitude_index ) ) + starting_offset, time_index );
        local[1][1][0] = z( int( gridIdx_( latitude_index + 1, longitude_index ) ) + starting_offset, time_index + 1 );
    }

    return AuvMath::Interpolate3D( epoch, latitude_degrees, longitude_degrees, local,
                                   float( forecastEpochSeconds_[time_index] ), float( forecastEpochSeconds_[time_index + 1] ),
                                   gridLat_( latitude_index, longitude_index ), gridLat_( latitude_index + 1, longitude_index + 1 ),
                                   gridLon_( latitude_index, longitude_index ), gridLon_( latitude_index + 1, longitude_index + 1 ) );
}

void HFRCMReconstructedInterpolator::updateExpansionCoefficients( void )
{
    int numForecastTimes = forecastTimesReader_->read1DArray( Units::EPOCH_SECOND, forecastEpochSeconds_ );
    expansionCoefficientsReader_->read2DClass( Units::NONE, expansionCoefficients_ );
    numModes_ = expansionCoefficients_.getM();
    numTimes_ = expansionCoefficients_.getN();
    if( ( numModes_ == HFRadarCompactModelForecasterIF::MAX_EOFS ) && ( numTimes_ == HFRadarCompactModelForecasterIF::TOTAL_HOURS ) && ( numForecastTimes == HFRadarCompactModelForecasterIF::TOTAL_HOURS ) )
    {
        forecastStart_ = Timestamp( forecastEpochSeconds_[ 0 ] );
        forecastEnd_ = Timestamp( forecastEpochSeconds_[ HFRadarCompactModelForecasterIF::TOTAL_HOURS - 1 ] ); // TODO: Is there a better/more flexible way to get the last element?
        haveValidForecast_ = true;
    }
    else
    {
        haveValidForecast_ = false; // TODO: Decide how to continue using previous forecast.
        logger_.syslog( "Error reading BlobValue for expansion coefficients.", Syslog::IMPORTANT );
    }
}

void HFRCMReconstructedInterpolator::reproject( void )
{
    currentField_ = eofs_ * expansionCoefficients_;//.subset( 0, -1, -pHours_ - 1, -1 );
    for( int i = 0 ; i < currentField_.getM(); ++i )
    {
        for( int j = 0; j < currentField_.getN(); ++j )
        {
            currentField_[i][j] += ensMean_( i, 0 ); // reconstruct in the full space
            currentField_[i][j] *= scalingFactors_( i, 0 ); // renormalize
        }
    }
}

bool HFRCMReconstructedInterpolator::getPreviousTimeIndex( int &time_index, double epoch )
{
    Timestamp ts( epoch );
    if( ( ts < forecastStart_ ) || ( ts > forecastEnd_ ) )
    {
        logger_.syslog( "Query for interpolated value " + ts.toString() + " outside the range of the data. [" + forecastStart_.toString() + ", " + forecastEnd_.toString() + "]", Syslog::DEBUG );
        return false;
    }
    else
    {
        double s0 = floorf( epoch / 3600.0 ) * 3600.0; // epoch seconds timestamp of the preceding hour
//        double dt = epoch - s0; // seconds since preceding hour

        // TODO: There is probably a more efficient way of finding the appropriate index.
        time_index = -1; // placeholder for index of preceding hour
        for( int j = 0; j < numTimes_; j++ )
        {
            if( forecastEpochSeconds_[j] == s0 )
            {
                time_index = j;
                if( debug_ ) logger_.syslog( "using time index " + Str( time_index ) + " for the hour preceding " + ts.toString() +  " ( forecastEpochSeconds_[" + Str( time_index ) + "] = " + Timestamp( forecastEpochSeconds_[time_index] ).toString() + " ) == ( s0 = " + Timestamp( s0 ).toString() + " )", Syslog::INFO );
                break;
            }
        }
        if( time_index != -1 )
        {
            return true;
        }
        else
        {
            logger_.syslog( "ran out of times: " + ts.toString(), Syslog::ERROR );
            return false;
        }
    }
}

// This may not be as efficient, but it is easier to read for me...
bool HFRCMReconstructedInterpolator::getLowerLeftCorner( int &latitude_index, int &longitude_index, const float latitude_degrees, const float longitude_degrees )
{
    if( latitude_degrees >= gridLat_( 0, 0 ) && latitude_degrees < gridLat_( -1, -1 ) && longitude_degrees >= gridLon_( 0, 0 ) && longitude_degrees < gridLon_( -1, -1 ) )
    {
        if( verbosity_ > 1 ) logger_.syslog( "requested location (" + Str( latitude_degrees ) + ", " + Str( longitude_degrees ) + ") is inside the bounding box", Syslog::DEBUG );
        latitude_index = -1;
        longitude_index = -1;
        for( int i = 0; i < gridLat_.getM() - 1; i++ )
        {
            if( verbosity_ > 3 ) logger_.syslog( "index " + Str( i ) + ": checking whether " + Str( latitude_degrees ) + " is between " + Str( gridLat_( i, 0 ) ) + " and " + Str( gridLat_( i + 1, 0 ) ), Syslog::DEBUG );
            if( ( gridLat_( i, 0 ) <= latitude_degrees ) && ( gridLat_( i + 1, 0 ) > latitude_degrees ) )
            {
                latitude_index = i;
                break;
            }
        }
        if( latitude_index == -1 )
        {
            logger_.syslog( "ran out of latitudes: ", latitude_degrees, Syslog::ERROR );
            return false;
        }
        for( int j = 0; j < gridLon_.getN() - 1; j++ )
        {
            if( verbosity_ > 3 ) logger_.syslog( "index " + Str( j ) + ": checking whether " + Str( longitude_degrees ) + " is between " + Str( gridLon_( 0, j ) ) + " and " + Str( gridLon_( 0, j + 1 ) ), Syslog::DEBUG );
            if( ( gridLon_( 0, j ) <= longitude_degrees ) && ( gridLon_( 0, j + 1 ) > longitude_degrees ) )
            {
                longitude_index = j;
                break;
            }
        }
        if( longitude_index == -1 )
        {
            logger_.syslog( "ran out of longitudes: ", longitude_degrees, Syslog::ERROR );
            return false;
        }
        return true;
    }
    else
    {
        logger_.syslog( "requested location (" + Str( latitude_degrees ) + ", " + Str( longitude_degrees ) + ") is outside the bounding box", Syslog::DEBUG );
        return false;
    }
}