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

#include "HFRCMTimeInterpolator.h"
#include "HFRCMTimeInterpolatorIF.h"

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

#include "data/ConfigReader.h"
// #include "data/UniversalDataWriter.h"
#include "data/BlobReader.h"
#include "data/Mtx.h"
#include "utils/Timestamp.h"

HFRCMTimeInterpolator* HFRCMTimeInterpolator::Instance_( NULL );

HFRCMTimeInterpolator::HFRCMTimeInterpolator( const Module* module )
    : SyncEstimationComponent( HFRCMTimeInterpolatorIF::NAME, module ),
      verbosity_( 0 ),
      expansionCoefficients_( logger_, HFRadarCompactModelForecasterIF::MAX_EOFS, HFRadarCompactModelForecasterIF::TOTAL_HOURS ), // initialize the Mtx to hold the expansion coefficients
//      expansionCoefficientsNow_( logger_, MAX_EOFS, 1 ), // initialize the Mtx to hold the expansion coefficients
      numModes_( 0 ),
      numTimes_( 0 ),
      forecastStart_( Timestamp::NOT_SET_TIME ),
      forecastEnd_( Timestamp::NOT_SET_TIME ),
      haveValidForecast_( false ),
      expansionCoefficientsReader_( newBlobReader( HFRadarCompactModelForecasterIF::FORECAST_EXPANSION_COEFFICIENTS ) ),
      forecastTimesReader_( newBlobReader( HFRadarCompactModelForecasterIF::FORECAST_TIMES ) )
{
    Instance_ = this;
}

HFRCMTimeInterpolator::~HFRCMTimeInterpolator()
{}

void HFRCMTimeInterpolator::initialize( void )
{
    logger_.syslog( "Initializing HFRCMTimeInterpolator component." );
    expansionCoefficientsReader_->requestData( true );
    forecastTimesReader_->requestData( true );
}

void HFRCMTimeInterpolator::run( void )
{
    if( expansionCoefficientsReader_->isActive() && expansionCoefficientsReader_->wasTouchedSinceLastRun( this ) )
    {
        // read the expansion coefficients from the new forecast
        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 );
        }
    }
    else if( haveValidForecast_ )
    {
        if( verbosity_ > 1 ) logger_.syslog( "no new forecast -- using existing expansion coefficients", Syslog::DEBUG );
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "no valid forecast", Syslog::DEBUG );
    }

    // If configured to do so, interpolate the ECs to the current time and write them out to the slate.
// TODO:    bool interpolationSuccessful = lookupExpansionCoefficients( Timestamp::Now(), expansionCoefficientsNow_ );
}

bool HFRCMTimeInterpolator::lookupExpansionCoefficients( Timestamp ts, Mtx& ec )
{

    if( !haveValidForecast_ )
    {
        if( verbosity_ > 0 ) logger_.syslog( "No valid forecast -- cannot look up expansion coefficients.", Syslog::DEBUG );
        return false; // Don't even have a valid forecast yet, so it can't interpolate.
    }
    else if( ( ts < forecastStart_ ) || ( ts > forecastEnd_ ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Query for interpolated value " + ts.toString() + " outside the range of the data. [" + forecastStart_.toString() + ", " + forecastEnd_.toString() + "]", Syslog::DEBUG );
        return false;
    }
    else
    {
        double es = ts.asDouble(); // epoch seconds
        double s0 = floorf( es / 3600.0 ) * 3600.0; // epoch seconds timestamp of the preceding hour
        double dt = es - s0; // seconds since preceding hour

        // TODO: There is probably a more efficient way of finding the appropriate index.
        int k = -1; // placeholder for index of preceding hour
        for( int j = 0; j < numTimes_; j++ )
        {
            if( forecastEpochSeconds_[j] == s0 )
            {
                k = j;
                break;
            }
        }

        if( k != -1 )
        {
            if( verbosity_ > 1 ) logger_.syslog( "using time index " + Str( k ) + " (" + Timestamp( forecastEpochSeconds_[k] ).toString() + ").", Syslog::DEBUG );
            for( int i = 0; i < numModes_; i++ )
            {
                float ec0 = expansionCoefficients_[i][k];
                float ec1 = expansionCoefficients_[i][k + 1];
                ec.set( i, 0, ec0 + ( ec1 - ec0 ) * ( dt / 3600.0 ) ); // TODO: Upgrade to something smoother than linear interpolation.
            }
            return true;
        }
        else
        {
            Timestamp ts0( s0 );
            logger_.syslog( "Could not find " + ts0.toString() + " in forecast from " + forecastStart_.toString() + " to " + forecastEnd_.toString() + ".", Syslog::ERROR );
            return false; // You didn't find an index with that timestamp.
        }
    }
}