/** \file
 *
 *  Contains the HFRadarModelCalc class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "HFRadarModelCalc.h"
#include "HFRadarModelCalcIF.h"

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

#include <cerrno>
#include <cstddef>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>

HFRadarModelCalc* HFRadarModelCalc::Instance_( NULL );

HFRadarModelCalc::HFRadarModelCalc( const Module* module )
    : SyncEstimationComponent( HFRadarModelCalcIF::NAME, module ),
      debug_( false ),
      eofs_( "Resources/HFRadarModel/1.U.mtx", logger_, 0, HFRadarModelCalcIF::MAX_EOFS ),
      ensMean_( "Resources/HFRadarModel/1.ens_mean.mtx", logger_ ),
      gridIdxRev_( "Resources/HFRadarModel/gr.hgrid.idx_x2X.mtx", logger_ ),
      gridX_( "Resources/HFRadarModel/gr.hgrid.X.mtx", logger_ ),
      gridY_( "Resources/HFRadarModel/gr.hgrid.Y.mtx", logger_ ),
      netB1_( "Resources/HFRadarModel/net.b1.mtx", logger_ ),
      ulags_( "Resources/HFRadarModel/net.forcingsHistory.mtx", logger_ ),
      netImpMean_( "Resources/HFRadarModel/net.inpMean.mtx", logger_ ),
      xlags_( "Resources/HFRadarModel/net.inputDataHistory.mtx", logger_ ),
      netTpca_( "Resources/HFRadarModel/net.Tpca.mtx", logger_ ),
      netW1_( "Resources/HFRadarModel/net.w1.mtx", logger_ ),
      stateInfoStdAll_( "Resources/HFRadarModel/stateInfo.std.all.mtx", logger_ ),
      dHours_( ( int )( xlags_( 0, -1 ) - xlags_( 0, 0 ) + 1 ) ),
      pHours_( HFRadarModelCalcIF::PROJECTION_HOURS ),
      gridIdx_( logger_, gridY_.getM(), gridX_.getN() ),
      data_( logger_, HFRadarModelCalcIF::MAX_EOFS, dHours_ + pHours_ ),
      st_( logger_, eofs_.getM(), pHours_ + 1 ),
      time_( new double[pHours_ + 1] ),
// TODO: initialize these      forecastEpochSeconds_( 0 ),
      forecastStartTime_( Timestamp::NOT_SET_TIME ),
      lat_( new float[gridY_.getM()] ),
      lon_( new float[gridX_.getN()] ),
      u_( new float * * [pHours_ + 1] ),
      v_( new float * * [pHours_ + 1] ),
      maxCurrent_( 0 ),
      ranOnce_( false ),
      calculationCompleteTime_( Timestamp::NOT_SET_TIME ),
      velocityAccuracy_( nan( "" ) ),
      latitudeReader_( newUniversalReader( UniversalURI::LATITUDE ) ),
      longitudeReader_( newUniversalReader( UniversalURI::LONGITUDE ) ),
      platformRommuniucationsReader_( newUniversalReader( UniversalURI::PLATFORM_COMMUNICATIONS ) ),
      forecastExpansionCoefficientsWriter_( newBlobWriter( HFRadarModelCalcIF::FORECAST_EXPANSION_COEFFICIENTS ) ),
      forecastTimesWriter_( newBlobWriter( HFRadarModelCalcIF::FORECAST_TIMES ) ),
      uWriter_( newUniversalWriter( UniversalURI::SURFACE_EASTWARD_SEA_WATER_VELOCITY ) ),
      vWriter_( newUniversalWriter( UniversalURI::SURFACE_NORTHWARD_SEA_WATER_VELOCITY ) ),
      // The velocities u should be North and v should be East for consistency with other internal coordinate frames -- change this when you come back to refactor the code here and make it more modular.
      velocityAccuracyConfigReader_( newConfigReader( HFRadarModelCalcIF::VELOCITY_ACCURACY ) )
{
    Instance_ = this;
    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;
    }
    for( int iLat = 0; iLat < gridY_.getM(); ++iLat )
    {
        lat_[iLat] = D2R( gridY_( iLat, 0 ) );
    }
    for( int iLon = 0; iLon < gridX_.getN(); ++iLon )
    {
        lon_[iLon] = D2R( gridX_( 0, iLon ) );
    }
    for( int iT = 0; iT <= pHours_; ++iT )
    {
        u_[iT] = new float*[gridY_.getM()];
        v_[iT] = new float*[gridY_.getM()];
        for( int iLat = 0; iLat < gridY_.getM(); ++iLat )
        {
            u_[iT][iLat] = new float[gridX_.getN()];
            v_[iT][iLat] = new float[gridX_.getN()];
            for( int iLon = 0; iLon < gridX_.getN(); ++iLon )
            {
                u_[iT][iLat][iLon] = u_[iT][iLat][iLon] = nanf( "" );
            }
        }
    }

    velocityAccuracyConfigReader_->read( Units::CENTIMETER_PER_SECOND, velocityAccuracy_ );
    uWriter_->setAccuracy( Units::CENTIMETER_PER_SECOND, velocityAccuracy_ );
    vWriter_->setAccuracy( Units::CENTIMETER_PER_SECOND, velocityAccuracy_ );
    //gridIdx_.write( "Data/HFRadarModel/grid.idx.mtx", logger_ );
}

HFRadarModelCalc::~HFRadarModelCalc()
{
    delete[] time_;
    delete[] lat_;
    delete[] lon_;
    for( int iT = 0; iT <= pHours_; ++iT )
    {
        for( int iLat = 0; iLat < gridY_.getM(); ++iLat )
        {
            delete[] u_[iT][iLat];
            delete[] v_[iT][iLat];
        }
        delete[] u_[iT];
        delete[] v_[iT];
    }
    delete[] u_;
    delete[] v_;
}

void HFRadarModelCalc::initialize( void )
{
    run();
}

void HFRadarModelCalc::run( void )
{
    if( !platformRommuniucationsReader_->wasTouchedSinceLastRun( this ) )
    {
        if( ranOnce_ ) publishSurfaceCurrentAtVehicleLocation();
        return;
    }
    Str hfRadarPath( "Data/HFRadarModel/" );
    int error = mkdir( hfRadarPath.cStr(), ACCESSPERMS );
    if( error && EEXIST != errno )
    {
        logger_.syslog( "Unable to access " + hfRadarPath, Syslog::ERROR );
        return;
    }

    DIR* hfRadarDir( opendir( hfRadarPath.cStr() ) );
    if( NULL == hfRadarDir )
    {
        logger_.syslog( "Unable to open " + hfRadarPath, Syslog::ERROR );
        return;
    }
    union // To use thread-safe readdir_r make sure dirent struct is big enough
    {
        struct dirent ent_;
        char b_[offsetof( struct dirent, d_name ) + NAME_MAX + 1];
    } dir;
    struct dirent* entry;
    int maxYear( 0 ), maxMonth( 0 ), maxDay( 0 ), maxHour( 0 );
    struct dirent* maxEntry( NULL );
    while( 0 == readdir_r( hfRadarDir, &dir.ent_, &entry ) && entry != NULL )
    {
        int year( 0 ), month( 0 ), day( 0 ), hour( 0 );
        if( 4 != sscanf( entry->d_name, "%04d%02d%02d%02d", &year, &month, &day, &hour ) )
        {
            continue;
        }
        if( year * 8928 + month * 744 + day * 24 + hour
                >= maxYear * 8928 + maxMonth * 744 + maxDay * 24 + maxHour )
        {
            maxYear = year;
            maxMonth = month;
            maxDay = day;
            maxHour = hour;
            maxEntry = entry;
        }
    }
    if( NULL != maxEntry ) // TODO: There is a better test in [HFRadarCompactModelForecaster] that will only update the calculation if *new* ECs are found. Backport that to this component if it stays around much longer.
    {
        forecastStartTime_ = Timestamp( maxYear, maxMonth, maxDay, maxHour, 0, 0 );
        for( int j = 0; j < dHours_; ++j )
        {
            int dHour( j - dHours_ + 1 );
            Timespan dt( dHour * 3600.0 );
            Timestamp inTime( forecastStartTime_ + dt );
            inTime.round( 3600 );
            char inName[] = "Data/HFRadarModel/yyyymmddhh";
            struct tm tm( inTime.asStructTm() );
            strftime( inName + sizeof( inName ) - 11, 11, "%Y%m%d%H", &tm );
            Mtx temp( ( char* )inName, logger_ );
            for( int i = 0; i < HFRadarModelCalcIF::MAX_EOFS; ++i )
            {
                data_[i][j] = temp.getM() > i && temp.getN() > 0 ? temp[i][0] : 0.0f;
            }
        }
        Mtx x( logger_, 1, HFRadarModelCalcIF::MAX_EOFS * xlags_.getN() + ulags_.getN(), 0.0f );
        for( int i = 0; i < pHours_; ++i ) // TODO: Consider switching to i-first indexing to be consistent with the code block above and avoid confusion.
        {
            for( int j = 0; j < xlags_.getN(); ++j )
            {
                int xInd = dHours_ + ( int )xlags_( 0, xlags_.getN() - j - 1 ) + i - 1;
                for( int k = 0; k < HFRadarModelCalcIF::MAX_EOFS; ++k )
                {
                    x[0][j * HFRadarModelCalcIF::MAX_EOFS + k] = data_( k, xInd );
                }
            }
            Mtx yf = glm2fwd( netImpMean_, netTpca_, netW1_, netB1_, x );
            for( int j = 0; j < HFRadarModelCalcIF::MAX_EOFS; ++j )
            {
                data_[j][dHours_ + i] = yf( 0, j );
            }
        }

        // add another (redundant) time in seconds, initialized as an array so that it can be written to the slate
        double forecastStartEpoch = forecastStartTime_.asDouble();
        for( int j = 0; j < dHours_ + pHours_; j++ )
        {
            forecastEpochSeconds_[j] = forecastStartEpoch + ( j - dHours_ + 1 ) * 3600.0;
        }
        // write the ECs and associated timestamps to the slate
        writeExpansionCoefficientsToSlate();

        st_ = eofs_ * data_.subset( 0, -1, -pHours_ - 1, -1 );
        for( int i = 0 ; i < st_.getM(); ++i )
        {
            for( int j = 0; j < st_.getN(); ++j )
            {
                st_[i][j] += ensMean_( i, 0 );//reconstruct in the full space
                st_[i][j] *= stateInfoStdAll_( i, 0 );//renormalize
            }
        }

        if( debug_ )
        {
            logger_.syslog( "st_ is " + Str( st_.getM() ) + "-by-" + Str( st_.getN() ), Syslog::INFO );
            logger_.syslog( "st_(0,0) = " + Str( st_( 0, 0 ) ) + "; st_(0,-1) = " + Str( st_( 0, -1 ) ), Syslog::INFO );
        }

        float maxCurrent2 = 0;
        for( int iT = 0; iT <= pHours_; ++iT )
        {
            for( int iLat = 0; iLat < gridY_.getM(); ++iLat )
            {
                for( int iLon = 0; iLon < gridX_.getN(); ++iLon )
                {
                    float gridIdx = ( gridIdx_[iLat][iLon] );
                    if( isnan( gridIdx ) )
                    {
                        u_[iT][iLat][iLon] = gridIdx;
                        v_[iT][iLat][iLon] = gridIdx;
                    }
                    else
                    {
                        int gridI = ( int ) gridIdx;
                        float u = st_[gridI][iT] * 0.01;
                        u_[iT][iLat][iLon] = u;
                        float v = st_[gridIdx + st_.getM() / 2][iT] * 0.01;
                        v_[iT][iLat][iLon] = v;
                        float current2 = u * u + v * v;
                        if( current2 > maxCurrent2 )
                        {
                            maxCurrent2 = current2;
                        }
                    }
                }
            }
        }
        maxCurrent_ = sqrt( maxCurrent2 );

        /*
        for( int i= 0 ; i < st_.getM(); ++i)
        {
        	for( int j=0; j < st_.getN(); ++j)
        	{
        		printf("(%d,%d)=%f ",i,j, st_(i,j));
        	}
        	printf("\n");
        }
        */

        for( int j = 0; j <= pHours_; ++j )
        {
            Timespan dt( j * 3600.0 );
            Timestamp inTime( forecastStartTime_ + dt );
            inTime.round( 3600 );
            time_[j] = inTime.asDouble();
        }

        ranOnce_ = true;
        calculationCompleteTime_ = Timestamp::Now();

        /*
        Mtx timeMtx( 1, pHours_ + 1 );
        for ( int j = 0; j <= pHours_; ++j )
        {
            timeMtx[0][j] = time_[j];
        }
        st_.write( "Data/HFRadarModel/u.mtx", logger_, 0, st_.getM() / 2 - 1 ); //save u part
        st_.write( "Data/HFRadarModel/v.mtx", logger_, st_.getM() / 2, -1 );  //save v part
        timeMtx.write( "Data/HFRadarModel/time.mtx", logger_ );  //save time
        */

        /*
        float u, v;
        lookup( u, v, 0, 36.8, -121.8 );
        printf( "u=%f,v=%f\n", u, v );
        */
    }
    closedir( hfRadarDir );
    publishSurfaceCurrentAtVehicleLocation();
}

void HFRadarModelCalc::publishSurfaceCurrentAtVehicleLocation( void )
{
    float latitude( nanf( "" ) ), longitude( nanf( "" ) );
    if( latitudeReader_->read( Units::DEGREE, latitude )
            && longitudeReader_->read( Units::DEGREE, longitude )
            && !isnan( latitude ) && !isnan( longitude ) )
    {
        float u( nanf( "" ) ), v( nanf( "" ) );
        Timestamp dataTimestamp( Timestamp::Now() );
        if( lookup( u, v, 0, latitude, longitude )
                && !isnan( u ) && !isnan( v ) )
        {
            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.
            uWriter_->setInvalid( false );
            vWriter_->setInvalid( false );
        }
        else
        {
            velocityAccuracy_ = nan( "" );
            u = nan( "" );
            v = nan( "" );
            uWriter_->setInvalid( true );
            vWriter_->setInvalid( true );
        }
        uWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, u, velocityAccuracy_, dataTimestamp );
        vWriter_->writeWithAccuracy( Units::CENTIMETER_PER_SECOND, v, velocityAccuracy_, dataTimestamp );
    }
}

Mtx HFRadarModelCalc::glm2fwd( const Mtx& netImpMean, const Mtx& netTpca, const Mtx& netW1,
                               const Mtx& netB1, const Mtx& buf )
{
    Mtx mtx( buf );
    mtx -= netImpMean;
    mtx = mtx * netTpca; // (1x463)*(463x428)= 1x428
    mtx = mtx * netW1; // (1x428)*(428xMAX_EOFS)=1xMAX_EOFS
    mtx += netB1;
    return mtx;
}

bool HFRadarModelCalc::lookup( float& u, float& v, const double dt, const float latDeg, const float lonDeg )
{
    if( !ranOnce_ )
    {
        return false;
    }

    double time = Timestamp::Now().asDouble() + dt;
    // logger_.syslog( "received request for time " + Timestamp( time ).toString(), Syslog::INFO );
    if( ( time < forecastEpochSeconds_[ 0 ] ) || ( time > forecastEpochSeconds_[ HFRadarModelCalcIF::TOTAL_HOURS - 1 ] ) )
    {
        Timestamp forecastStart( forecastEpochSeconds_[ 0 ] );
        Timestamp forecastEnd( forecastEpochSeconds_[ HFRadarModelCalcIF::TOTAL_HOURS - 1 ] );
        Timestamp ts( time );
        logger_.syslog( "Query for interpolated value " + ts.toString() + " outside the range of the data. [" + forecastStart.toString() + ", " + forecastEnd.toString() + "]", Syslog::DEBUG );
        u = nanf( "" );
        v = nanf( "" );
        return false;
    }

    // logger_.syslog( "received request for location (" + Str( latDeg ) + ", " + Str( lonDeg ) + ")", Syslog::INFO );
    if( ( latDeg < gridY_( 0, 0 ) ) || ( latDeg >= gridY_( -1, -1 ) ) || ( lonDeg < gridX_( 0, 0 ) ) || ( lonDeg >= gridX_( -1, -1 ) ) )
    {
        logger_.syslog( "requested location (" + Str( latDeg ) + ", " + Str( lonDeg ) + ") is outside the bounding box", Syslog::DEBUG );
        u = nanf( "" );
        v = nanf( "" );
        return false;
    }

    int tIndex( 0 );
    for( ; time_[tIndex + 1] <= time && tIndex + 1 < pHours_; ++tIndex );
    int latIndex( 0 );
    for( ; gridY_( latIndex + 1, 0 ) <= latDeg && latIndex + 2 < gridY_.getM(); ++latIndex );
    int lonIndex( 0 );
    for( ; gridX_( 0, lonIndex + 1 ) <= lonDeg && lonIndex + 2 < gridX_.getN(); ++lonIndex );

    if( debug_ ) logger_.syslog( "time starting: " + Timestamp( time_[tIndex] ).toString() + "; st_(0," + Str( tIndex ) + ") = " + Str( st_( 0, tIndex ) ), Syslog::INFO );

    //printf( "tIndex:%d, latIndex:%d, lonIndex:%d\n", tIndex, latIndex, lonIndex );

    float gridIdx[4];
    bool allNan( true );
    bool someNan( false );
    for( int i = 0; i < 4; ++i )
    {
        gridIdx[i] = ( gridIdx_[latIndex + ( i & 0x02 ? 1 : 0 )][lonIndex + ( i & 0x01 ? 1 : 0 )] );
        allNan &= isnan( gridIdx[i] );
        someNan |= isnan( gridIdx[i] );
    }
    //printf( "gridIdx: %f,%f,%f,%f\n", gridIdx[0], gridIdx[1], gridIdx[2], gridIdx[3] );
    if( allNan )
    {
        u = nanf( "" );
        v = nanf( "" );
        logger_.syslog( "all grid points nearby have NaN in them", Syslog::DEBUG );
        return false;
    }
    if( someNan )
    {
        float closestDist( 1e10 );
        float closestGridIdx( -1 );
        for( int i = 0; i < 4; ++i )
        {
            float dist = isnan( gridIdx[i] ? nanf( "" ) : Location::GetDistanceFast( latDeg, lonDeg, gridY_( latIndex + ( i & 0x02 ? 1 : 0 ), 0 ), gridX_( 0, lonIndex + ( i & 0x01 ? 1 : 0 ) ) ) );
            if( !isnan( dist ) && dist < closestDist )
            {
                closestDist = dist;
                closestGridIdx = gridIdx[i];
            }
        }
        if( closestGridIdx < 0 )
        {
            u = nanf( "" );
            v = nanf( "" );
            logger_.syslog( "index for closest grid point evaluated to negative", Syslog::DEBUG );
            return false;
        }
        for( int i = 0; i < 4; ++i )
        {
            if( isnan( gridIdx[i] ) )
            {
                gridIdx[i] = closestGridIdx;
            }
        }
        //printf( "gridIdx: %f,%f,%f,%f\n", gridIdx[0], gridIdx[1], gridIdx[2], gridIdx[3] );
    }
    float uData[2][2][2];
    float vData[2][2][2];
    for( int iT = 0; iT <= 1; ++iT )
    {
        for( int iLat = 0; iLat <= 1; ++iLat )
        {
            for( int iLon = 0; iLon <= 1; ++iLon )
            {
                float gridI = gridIdx[iLat * 2 + iLon];
                if( isnan( gridI ) || gridI < 0 )
                {
                    uData[iT][iLat][iLon] = vData[iT][iLat][iLon] = NAN;
                }
                else
                {
                    uData[iT][iLat][iLon] = st_[( int )gridI][tIndex + iT];
                    vData[iT][iLat][iLon] = st_[( int )gridI + st_.getM() / 2][tIndex + iT];
                }
                //printf( "u(%d,%d,%d)=%f", iT, iLat, iLon, uData[iT][iLat][iLon] );
                //printf( ", v(%d,%d,%d)=%f\n", iT, iLat, iLon, vData[iT][iLat][iLon] );
            }
        }
    }

    //printf( "Time: %f <= %f <% f\n", ( float )time_[tIndex], time, ( float )time_[tIndex+1] );
    //printf( "Lat: %f <= %f < %f\n", gridY_(( int )latIndex, 0 ), lat, gridY_(( int )latIndex + 1, 0 ) );
    //printf( "Lon: %f <= %f < %f\n", gridX_( 0, ( int )lonIndex ), lon, gridX_( 0, ( int )lonIndex + 1 ) );

    u = AuvMath::Interpolate3D( ( float )time, latDeg, lonDeg, uData,
                                ( float )time_[tIndex], ( float )time_[tIndex + 1],
                                gridY_( ( int )latIndex, 0 ), gridY_( ( int )latIndex + 1, 0 ),
                                gridX_( 0, ( int )lonIndex ), gridX_( 0, ( int )lonIndex + 1 ) );

    v = AuvMath::Interpolate3D( ( float )time, latDeg, lonDeg, vData,
                                ( float )time_[tIndex], ( float )time_[tIndex + 1],
                                gridY_( ( int )latIndex, 0 ), gridY_( ( int )latIndex + 1, 0 ),
                                gridX_( 0, ( int )lonIndex ), gridX_( 0, ( int )lonIndex + 1 ) );

    return true;
}

void HFRadarModelCalc::writeExpansionCoefficientsToSlate( void )
{
    Timestamp calculationCompleteTime = Timestamp::Now();
    Mtx expansionCoefficients_ = data_; // TODO: actually change the variable names to avoid this copy operation if this component sticks around much longer
    logger_.syslog( "Forecast time " + forecastStartTime_.toSmallString() + " published", Syslog::IMPORTANT );
    forecastExpansionCoefficientsWriter_->write2DClass( Units::NONE, expansionCoefficients_, calculationCompleteTime );
    forecastTimesWriter_->write1DArray( Units::EPOCH_SECOND, forecastEpochSeconds_, calculationCompleteTime );
    if( debug_ ) logger_.syslog( "expansionCoefficients_[0][" + Str( HFRadarModelCalcIF::HISTORY_HOURS - 1 ) + "] = " + Str( expansionCoefficients_[0][HFRadarModelCalcIF::HISTORY_HOURS - 1] ) + " for " + Timestamp( forecastEpochSeconds_[HFRadarModelCalcIF::HISTORY_HOURS - 1] ).toString(), Syslog::IMPORTANT );
}