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

#include "HFRCMSpaceInterpolator.h"
#include "HFRCMSpaceInterpolatorIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/Mtx.h"
#include "data/UniversalDataReader.h"

HFRCMSpaceInterpolator* HFRCMSpaceInterpolator::Instance_( NULL );

HFRCMSpaceInterpolator::HFRCMSpaceInterpolator( const Module* module )
    : SyncEstimationComponent( HFRCMSpaceInterpolatorIF::NAME, module ),
      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 ),
      latitudeReader_( newUniversalReader( UniversalURI::LATITUDE ) ),
      longitudeReader_( newUniversalReader( UniversalURI::LONGITUDE ) ),
      verbosityLevelConfigReader_( newConfigReader( HFRCMSpaceInterpolatorIF::VERBOSITY ) )
{
    Instance_ = this;
}

HFRCMSpaceInterpolator::~HFRCMSpaceInterpolator()
{}

void HFRCMSpaceInterpolator::initialize( void )
{
    verbosityLevelConfigReader_->read( Units::COUNT, verbosity_ );
    logger_.syslog( "Initializing HFRCMSpaceInterpolator component with verbosity level " + Str( verbosity_ ) + "." );

    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 HFRCMSpaceInterpolator::run( void )
{
    // TODO: If configured to do so, periodically publish the EOFs at the vehicle's current location to the slate.
}

bool HFRCMSpaceInterpolator::lookupEmpiricalOrthogonalFunctions( Mtx& eastEOFs, Mtx& northEOFs, float& eastEM,  float& northEM, float& eastSF,  float& northSF, 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( ( eastEOFs.getN() != numModes_ ) || ( northEOFs.getN() != numModes_ ) ) // check sizes of inputs and outputs
    {
        logger_.syslog( "Output arrays do not have enough slots for all EOF modes. (Need " + Str( numModes_ ) + ". East has " + Str( eastEOFs.getN() ) + ", North has " + Str( northEOFs.getN() ) + ".)", Syslog::DEBUG );
        return false; // TODO: resize the output arrays instead
    }
    int latitude_index( -1 ), longitude_index( -1 );
    if( getLowerLeftCorner( latitude_index, longitude_index, latitude_degrees, longitude_degrees ) )
    {
        if( verbosity_ > 1 ) logger_.syslog( "found lower left corner at indices: " + Str( latitude_index ) + ", " + Str( longitude_index ), Syslog::INFO );
        if( verbosity_ > 2 )
        {
            int lower_left_index( gridIdx_[latitude_index][longitude_index] );
            int lower_right_index( gridIdx_[latitude_index][longitude_index + 1] );
            int upper_right_index( gridIdx_[latitude_index + 1][longitude_index + 1] );
            int upper_left_index( gridIdx_[latitude_index + 1][longitude_index] );
            logger_.syslog( "LL: " + Str( lower_left_index ) + ", LR: " + Str( lower_right_index ) + ", UR: " + Str( upper_right_index ) + ", UL: " + Str( upper_left_index ), Syslog::INFO );
        }
        // find the appropriate scale factors
        eastSF = scalingFactors_( 0, 0 );
        northSF = scalingFactors_( scalingFactors_.getM() - 1, 0 );
        // XXX shortcut since the scaling factors are constant for the two halves
        eastEM = interpolateRaveledTwoDimensionalGrid( ensMean_, latitude_degrees, longitude_degrees, latitude_index, longitude_index );
        northEM = interpolateRaveledTwoDimensionalGrid( ensMean_, latitude_degrees, longitude_degrees, latitude_index, longitude_index, numLocations_ );
        for( int m = 0; m < numModes_; m++ )
        {
            eastEOFs.set( 0, m, interpolateRaveledTwoDimensionalGrid( eofs_, latitude_degrees, longitude_degrees, latitude_index, longitude_index, 0, m ) );
            northEOFs.set( 0, m, interpolateRaveledTwoDimensionalGrid( eofs_, latitude_degrees, longitude_degrees, latitude_index, longitude_index, numLocations_, m ) );
        }
        return true;
    }
    else
    {
        if( verbosity_ > 1 ) logger_.syslog( "did not find lower left corner", Syslog::INFO );
        eastEM = nan( "" );
        northEM = nan( "" );
        eastSF = nan( "" );
        northSF = nan( "" );
        for( int m = 0; m < numModes_; m++ )
        {
            eastEOFs.set( 0, m, nan( "" ) );
            northEOFs.set( 0, m, nan( "" ) );
        }
        return false;
    }
}

// This may not be as efficient, but it is easier to read for me...
bool HFRCMSpaceInterpolator::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;
    }
}

float HFRCMSpaceInterpolator::interpolateRaveledTwoDimensionalGrid( const Mtx z, const float latitude_degrees, const float longitude_degrees, const int latitude_index, const int longitude_index, const int starting_offset, const int layer )
{
    // TODO: Consider making a GeographicalGrid class that provides class methods for interpolation. It could be useful for this Compact Model work as well as for the bathymetry charts, magnetic corrections, etc.
    // TODO: pass subset args (e.g., eastEOFs_, northEOFs_) in the future, instead of using starting_offset
    /*
    int lower_left_index( gridIdx_[latitude_index][longitude_index] );
    int lower_right_index( gridIdx_[latitude_index][longitude_index + 1] );
    int upper_right_index( gridIdx_[latitude_index + 1][longitude_index + 1] );
    int upper_left_index( gridIdx_[latitude_index + 1][longitude_index] );
    */
    float local[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::Interpolate2D
    if( !isnan( gridIdx_[latitude_index][longitude_index] ) )
    {
        local[0][0] = z( int( gridIdx_( latitude_index, longitude_index ) ) + starting_offset, layer );
    }
    if( !isnan( gridIdx_( latitude_index, longitude_index + 1 ) ) )
    {
        local[0][1] = z( int( gridIdx_( latitude_index, longitude_index + 1 ) ) + starting_offset, layer );
    }
    if( !isnan( gridIdx_( latitude_index + 1, longitude_index + 1 ) ) )
    {
        local[1][1] = z( int( gridIdx_( latitude_index + 1, longitude_index + 1 ) ) + starting_offset, layer );
    }
    if( !isnan( gridIdx_( latitude_index + 1, longitude_index ) ) )
    {
        local[1][0] = z( int( gridIdx_( latitude_index + 1, longitude_index ) ) + starting_offset, layer );
    }
    /* can't do it this simple way because gridIdx_ can have NaN -- still might be simpler to fill each of the EOFs into a zero-padded array instead of checking all of these conditionals

        local[0][0] = z( int( gridIdx_( latitude_index, longitude_index ) ) + starting_offset, layer );
        local[1][0] = z( int( gridIdx_( latitude_index, longitude_index + 1 ) ) + starting_offset, layer );
        local[1][1] = z( int( gridIdx_( latitude_index + 1, longitude_index + 1 ) ) + starting_offset, layer );
        local[0][1] = z( int( gridIdx_( latitude_index + 1, longitude_index ) ) + starting_offset, layer );
    */
    return AuvMath::Interpolate2D( latitude_degrees, longitude_degrees, local, gridLat_( latitude_index, longitude_index ), gridLat_( latitude_index + 1, longitude_index + 1 ), gridLon_( latitude_index, longitude_index ), gridLon_( latitude_index + 1, longitude_index + 1 ) );
    // TODO: test possibly minute differences in results below
    // return AuvMath::Interpolate2D( latitude_degrees, longitude_degrees, local, gridLat_( latitude_index, 0 ), gridLat_( latitude_index + 1, 0 ), gridLon_( 0, longitude_index ), gridLon_( 0, longitude_index + 1 ) );
    // TODO: Perform explicit bilinear interpolation based on distance (over the globe) to each grid point.
}