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

#include "InternalEnvSim.h"

#include "data/ConfigReader.h"
#include "data/StrValue.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "units/UnitRegistry.h"
#include "utils/Datum.h"

// To get the relevant names for shared vars
#include "InternalEnvSimIF.h"

InternalEnvSim::VarData::VarData()
    : var_( NULL )
{
    for( int i = 0; i < 4; ++i )
    {
        indices_[i] = -1;
    }

    for( int i = 0; i < 2; ++i )
    {
        for( int j = 0; j < 2; ++j )
        {
            for( int k = 0; k < 2; ++k )
            {
                for( int l = 0; l < 2; ++l )
                {
                    array_[i][j][k][l] = 0.0f;
                }
            }
        }
    }
}


InternalEnvSim::InternalEnvSim( const Module* module )
    : SyncSimulatorComponent( InternalEnvSimIF::NAME, module ),
      debug_( false ),                       // Flag
      ok_( false ),                          // Status
      netCdf3Reader_( NULL ),                // Gridded data file reader
      nc3FileCfgSetting_( Str::EMPTY_STR ),  // filename of file that contains ocean model run
      varNameCfgSettings_( true ),           // names of variables in ocean model data
      varUnitCfgSettings_( false ),          // units of variables in ocean model data
      cfgAtts_(),                            // global attribute overrides in ocean model data
      timeAdjustCfgSetting_( nanf( "" ) ),   // If non-NaN, put start of vehicle logs this far into simulation time span
      modelTime_( NULL ),                    // time array for netCdf3Reader
      modelTimeSize_( 0 ),                   // size of time Array
      modelDepth_( NULL ),                   // depth array for netCdf3Reader
      modelDepthSize_( 0 ),                  // size of depth Array
      modelLatitude_( NULL ),                // latitude array for netCdf3Reader
      modelLatitudeSize_( 0 ),               // size of latitude Array
      modelLongitude_( NULL ),               // longitude array for netCdf3Reader
      modelLongitudeSize_( 0 ),              // size of longitude Array
      varLatitude_( NULL ),
      varLongitude_( NULL ),
      modelY_( NULL ),                       // projection Y array for netCdf3Reader
      modelYSize_( 0 ),                      // size of projection Y Array
      modelX_( NULL ),                       // projection X array for netCdf3Reader
      modelXSize_( 0 ),                      // size of projection X Array
      vars_( NULL ),                         // Variables
      gridMapping_( GRID_MAPPING_NONE ),     // Coordinate mapping transformation name
      scaleCentralMeridian_( 1.0 ),          // Coordinate mapping parameter
      lonCentralMeridian_( 0.0 ),            // Coordinate mapping parameter
      lonProjectionOrigin_( 0.0 ),           // Coordinate mapping parameter
      latProjectionOrigin_( 0.0 ),           // Coordinate mapping parameter
      falseEasting_( 0.0 ),                  // Coordinate mapping parameter
      falseNorthing_( 0.0 ),                 // Coordinate mapping parameter
      decompress_( NULL )                    // Array to speed decompression of compressed data
{
    // Init config readers
    nc3FileCfgReader_ = newConfigReader( InternalEnvSimIF::NC3_FILE );
    for( int iVar = 0; iVar < InternalEnvSimIF::MAX_VARS; ++iVar )
    {
        varCfgReaders_[iVar] = newConfigReader( *InternalEnvSimIF::VARS[iVar] );
    }
    for( int iAtt = 0; iAtt < InternalEnvSimIF::MAX_ATTS; ++iAtt )
    {
        attCfgReaders_[iAtt] = newConfigReader( *InternalEnvSimIF::ATTS[iAtt] );
    }
    timeAdjustCfgReader_ = newConfigReader( InternalEnvSimIF::TIME_ADJUST );

    // Depth slate readers and writers
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    // GPS slate readers and writers
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

}

/// Initialize
void InternalEnvSim::initialize( void )
{

    logger_.syslog( " InternaEnvlSim initializing..." );
    ok_ = true;

    StrValue nc3FileTmp;
    ok_ = nc3FileCfgReader_->read( nc3FileTmp );
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Could not read name of Netcdf3 file" ), Syslog::CRITICAL );
        return;
    }
    nc3FileCfgSetting_ = nc3FileTmp.asString();

    for( int iVar = 0; iVar < InternalEnvSimIF::MAX_VARS; ++iVar )
    {
        StrValue varTmp;
        if( varCfgReaders_[iVar]->read( varTmp ) && varTmp.asString().length() > 0 )
        {
            size_t colonAt = varTmp.asString().find( ':' );
            if( colonAt == Str::NO_POS || colonAt == 0 || colonAt == varTmp.asString().length() - 1 )
            {
                ok_ = false;
                logger_.syslog( Str( "Misplaced colon in var:unit string: " + varTmp.asString() ), Syslog::CRITICAL );
                return;
            }
            Str unitStr = varTmp.asString().substr( colonAt + 1 );
            const Unit* unit = UnitRegistry::FindUnit( unitStr );
            if( NULL == unit )
            {
                ok_ = false;
                logger_.syslog( Str( "Unkonwn unit in var:unit string: " + unitStr ), Syslog::CRITICAL );
                return;
            }
            varUnitCfgSettings_.push( unit );
            varNameCfgSettings_.push( new Str( varTmp.asString().substr( 0, colonAt ) ) );
        }
    }

    for( int iAtt = 0; iAtt < InternalEnvSimIF::MAX_VARS; ++iAtt )
    {
        StrValue attTmp;
        if( attCfgReaders_[iAtt]->read( attTmp ) && attTmp.asString().length() > 0 )
        {
            size_t colonAt = attTmp.asString().find( ':' );
            if( colonAt == Str::NO_POS || colonAt == 0 || colonAt == attTmp.asString().length() - 1 )
            {
                ok_ = false;
                logger_.syslog( Str( "Misplaced colon in att:value string: " + attTmp.asString() ), Syslog::CRITICAL );
                return;
            }
            double value = StrToDouble( attTmp.asString().cStr() + colonAt + 1, attTmp.asString().length() - colonAt );
            NetCdf3::NetCdfAtt* att = new NetCdf3::NetCdfAtt;
            att->nelems_ = 1;
            att->netCdfType_ = NetCdf3::NC_DOUBLE;
            att->values_ = new char[sizeof( double )];
            memcpy( att->values_, &value, sizeof( double ) );
            cfgAtts_.put( attTmp.asString().substr( 0, colonAt ), att, true );
        }
    }

    timeAdjustCfgReader_->read( Units::SECOND, timeAdjustCfgSetting_ );

    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine.\n" ), Syslog::CRITICAL );
        return;
    }

    configureSensors();
}

/// Run
void InternalEnvSim::run( void )
{
    if( !ok_ )
    {
        return ;
    }
    float values[InternalEnvSimIF::MAX_VARS];
    double dTime( timeOfRun_.asDouble() );
    double depthM( nanf( "" ) ), latDeg( nanf( "" ) ), lonDeg( nanf( "" ) );

    if( !depthReader_->read( Units::METER, depthM ) || isnan( depthM )
            || !latitudeReader_->read( Units::DEGREE, latDeg ) || isnan( latDeg )
            || !longitudeReader_->read( Units::DEGREE, lonDeg ) || isnan( lonDeg )
      )
    {
        return;
    }

    for( unsigned int iVar = 0; iVar < varNameCfgSettings_.size(); ++iVar )
    {
        values[iVar] = nanf( "" );
    }

    simulateSensors( dTime, depthM, latDeg, lonDeg, values );

    for( unsigned int iVar = 0; iVar < varNameCfgSettings_.size(); ++iVar )
    {
        if( debug_ )printf( "Writing %s =%g%s\n", varNameCfgSettings_[iVar]->cStr(), values[iVar], varUnitCfgSettings_[iVar]->getName() );
        SimSlate::WriteScience( *varNameCfgSettings_[iVar], *varUnitCfgSettings_[iVar], values[iVar] );
    }
}

/// To be replaced by Str::toDouble() when it becomes available from LBL branch
double InternalEnvSim::StrToDouble( const char* str, size_t len )
{
    if( len == Str::NO_POS )
    {
        len = strlen( str );
    }

    if( len == 0 || str[0] == 'n' )
    {
        return nanf( "" );
    }
    else
    {
        return strtof( str, NULL );
    }
}

/// Deinitialize
void InternalEnvSim::uninitialize()
{
    if( NULL != netCdf3Reader_ )
    {
        delete[] modelTime_;
        delete[] modelDepth_;
        delete[] modelLatitude_;
        delete[] modelLongitude_;
        delete netCdf3Reader_;
        netCdf3Reader_ = NULL;
    }
}

void InternalEnvSim::configureSensors()
{
    netCdf3Reader_ = NetCdf3Reader::NewNetCdf3Reader( nc3FileCfgSetting_.cStr() );
    if( NULL == netCdf3Reader_ )
    {
        nc3FileCfgSetting_ = "../" + nc3FileCfgSetting_;
        netCdf3Reader_ = NetCdf3Reader::NewNetCdf3Reader( nc3FileCfgSetting_.cStr() );
    }
    if( NULL == netCdf3Reader_ )
    {
        logger_.syslog( "Error -- could not open netCDF file at " + nc3FileCfgSetting_, Syslog::CRITICAL );
        ok_ = false;
        return;
    }

    //logger_.syslog( "Opened ", nc3FileCfgSetting_ );
    printf( "Opened %s\n", nc3FileCfgSetting_.cStr() );
    const NetCdf3Reader::NetCdfDimArray& netCdf3DimArray = netCdf3Reader_->getDimArray();
    NetCdf3Reader::NetCdfVar* varTime( netCdf3Reader_->findNetCdfVarByAttribute( "standard_name", "time" )/*->findVar( "time" )*/ );
    if( NULL != varTime )
    {
        modelTimeSize_ = ( netCdf3DimArray.get( varTime->dimIds_[ 0 ] ) )->dimSize_;
        modelTime_ = new float[ modelTimeSize_ ];
        netCdf3Reader_->read1DArray( modelTime_, NetCdf3Reader::NC_FLOAT, *varTime, 0, modelTimeSize_ - 1 );
        /*for( unsigned int i = 0; i < modelTimeSize_; ++i )
        {
          printf("modelTime[%d] is %g\n", i,  modelTime_[i] );
        }*/
        if( !isnan( timeAdjustCfgSetting_ ) )
        {
            double startTime = Timestamp::Now().asDouble() - timeAdjustCfgSetting_;
            double timeAdjust( nanf( "" ) );
            for( unsigned int i = 0; i < modelTimeSize_; ++i )
            {
                if( isnan( timeAdjust ) )
                {
                    timeAdjust = modelTime_[0] - startTime;
                }
                modelTime_[i] -= timeAdjust;
            }
        }
    }

    NetCdf3Reader::NetCdfVar* varDepth = netCdf3Reader_->findNetCdfVarByAttribute( "standard_name", "depth" );//->findVar( "depth" );
    if( NULL != varDepth )
    {
        modelDepthSize_ = ( netCdf3DimArray.get( varDepth->dimIds_[ 0 ] ) )->dimSize_;
        modelDepth_ = new float[ modelDepthSize_ ];
        netCdf3Reader_->read1DArray( modelDepth_, NetCdf3Reader::NC_FLOAT, *varDepth, 0, modelDepthSize_ - 1 );
    }

    NetCdf3Reader::NetCdfVar* varY = netCdf3Reader_->findNetCdfVarByAttribute( "standard_name", "projection_y_coordinate" );
    NetCdf3Reader::NetCdfVar* varX = netCdf3Reader_->findNetCdfVarByAttribute( "standard_name", "projection_x_coordinate" );
    if( NULL != varX && NULL != varY )
    {
        NetCdf3Reader::NetCdfAtt* attGridMapping = netCdf3Reader_->findAtt( "grid_mapping_name" );
        if( NULL != attGridMapping )
        {
            Str gridMapping;
            if( netCdf3Reader_->readAttStr( gridMapping, attGridMapping ) )
            {
                if( gridMapping == "azimuthal_equidistant" )
                {
                    gridMapping_ = GRID_MAPPING_AZIMUTHAL_EQUIDISTANT;
                }
                else if( gridMapping == "transverse_mercator" )
                {
                    gridMapping_ = GRID_MAPPING_TRANSVERSE_MERCATOR;
                }
                else
                {
                    fprintf( stderr, "Unhandled grid mapping name: %s\n", gridMapping.cStr() );
                }
            }
            modelXSize_ = ( netCdf3DimArray.get( varX->dimIds_[ 0 ] ) )->dimSize_;
            modelX_ = new float[ modelXSize_ ];
            netCdf3Reader_->read1DArray( modelX_, NetCdf3Reader::NC_FLOAT, *varX, 0, modelXSize_ - 1 );

            modelYSize_ = netCdf3DimArray.get( varY->dimIds_[ 0 ] )->dimSize_;
            modelY_ = new float[ modelYSize_ ];
            netCdf3Reader_->read1DArray( modelY_, NetCdf3Reader::NC_FLOAT, *varY, 0, modelYSize_ - 1 );

            NetCdf3Reader::NetCdfVar* varCompressYX = netCdf3Reader_->findNetCdfVarByAttribute( "compress", "y x" );
            if( NULL != varCompressYX )
            {
                int compressSize = netCdf3DimArray.get( varCompressYX->dimIds_[ 0 ] )->dimSize_;
                int* compress = new int[ compressSize ];
                netCdf3Reader_->read1DArray( compress, NetCdf3Reader::NC_INT, *varCompressYX, 0, compressSize - 1 );
                int decompress_size = modelXSize_ * modelYSize_;
                decompress_ = new int[ decompress_size ];
                for( int i = 0; i < decompress_size ; ++i )
                {
                    decompress_[i] = -1;
                }
                if( debug_ )
                {
                    printf( "compressSize=%d, decompress_size=%d\n", compressSize, decompress_size );
                }
                for( int i = 0; i < compressSize ; ++i )
                {
                    int index = compress[i];
                    if( debug_ )
                    {
                        if( i < 10 || ( i >= 100 && i <= 110 ) || ( i >= 1000 && i <= 1010 ) || ( i >= 10000 && i <= 10010 ) || ( i >= 100000 && i <= 100010 ) )
                        {
                            printf( "Set decompress[%d]=%d\n", index, i );
                        }
                    }
                    if( index >= 0 && index < decompress_size )
                    {
                        decompress_[index] = i;
                    }
                }
                delete[] compress;
            }
        }
        NetCdf3Reader::NetCdfAtt* attScaleCentralMeridian = findAtt( netCdf3Reader_, "scale_factor_at_central_meridian" );
        if( NULL != attScaleCentralMeridian )
        {
            netCdf3Reader_->readAtt( &scaleCentralMeridian_, NetCdf3::NC_DOUBLE, attScaleCentralMeridian );
        }
        NetCdf3Reader::NetCdfAtt* attLonCentralMeridian = findAtt( netCdf3Reader_, "longitude_of_central_meridian" );
        if( NULL != attLonCentralMeridian )
        {
            netCdf3Reader_->readAtt( &lonCentralMeridian_, NetCdf3::NC_DOUBLE, attLonCentralMeridian );
        }
        if( debug_ ) logger_.syslog( "Longitude of central meridian = ", lonCentralMeridian_, Syslog::INFO );
        NetCdf3Reader::NetCdfAtt* attLonProjectionOrigin = findAtt( netCdf3Reader_, "longitude_of_projection_origin" );
        if( NULL != attLonProjectionOrigin )
        {
            netCdf3Reader_->readAtt( &lonProjectionOrigin_, NetCdf3::NC_DOUBLE, attLonProjectionOrigin );
        }
        NetCdf3Reader::NetCdfAtt* attLatProjectionOrigin = findAtt( netCdf3Reader_, "latitude_of_projection_origin" );
        if( NULL != attLatProjectionOrigin )
        {
            netCdf3Reader_->readAtt( &latProjectionOrigin_, NetCdf3::NC_DOUBLE, attLatProjectionOrigin );
        }
        NetCdf3Reader::NetCdfAtt* attFalseEasting = findAtt( netCdf3Reader_, "false_easting" );
        if( NULL != attFalseEasting )
        {
            netCdf3Reader_->readAtt( &falseEasting_, NetCdf3::NC_DOUBLE, attFalseEasting );
        }
        if( debug_ ) logger_.syslog( "False easting = ", falseEasting_, Syslog::INFO );
        NetCdf3Reader::NetCdfAtt* attFalseNorthing = findAtt( netCdf3Reader_, "false_northing" );
        if( NULL != attFalseNorthing )
        {
            netCdf3Reader_->readAtt( &falseNorthing_, NetCdf3::NC_DOUBLE, attFalseNorthing );
        }
        if( debug_ ) logger_.syslog( "False northing = ", falseNorthing_, Syslog::INFO );
    }
    else
    {
        gridMapping_ = GRID_MAPPING_NONE;
    }
    if( debug_ )
    {
        logger_.syslog( "gridMapping=", gridMapping_, Syslog::INFO );
    }
    if( varNameCfgSettings_.size() > 0 )
    {
        vars_ = new VarData[varNameCfgSettings_.size()];
        for( unsigned int i = 0; i < varNameCfgSettings_.size(); ++i )
        {
            vars_[i].var_ = netCdf3Reader_->findNetCdfVarByAttribute( "standard_name", varNameCfgSettings_[i]->cStr() );
            if( debug_ && vars_[i].var_ != NULL ) logger_.syslog( "Found " + *varNameCfgSettings_[i], Syslog::INFO );
            if( vars_[i].var_ == NULL )
            {
                logger_.syslog( "Could not find variable with standard_name=" + *varNameCfgSettings_[i], Syslog::CRITICAL );
            }
        }
    }
}

void InternalEnvSim::azimuthalEquidistantToCoordinates( double &Ym, double &Xm, double depLoc, double latDeg, double lonDeg )
{
    AzimuthalEquidistantToCoordinates( Ym,  Xm, depLoc, latDeg, lonDeg,
                                       latProjectionOrigin_, lonProjectionOrigin_ );
}

void InternalEnvSim::AzimuthalEquidistantToCoordinates( double &Ym, double &Xm, double depLoc, double latDeg, double lonDeg,
        double latProjectionOrigin, double lonProjectionOrigin )
{
    //WGS84 Ellipsoid coefficients
    //
    //major axis (equatorial radius, in meters)
    const double a = 6378137;
    //minor axis (polar radius, in meters)
    const double b = 6356752.314245;

    double latLoc = D2R( latProjectionOrigin );
    double lonLoc = D2R( lonProjectionOrigin );
    double lat = D2R( latDeg );
    double lon = D2R( lonDeg );

    //local radius (center to surface, in meters)
    double r = 1 / sqrt( pow( cos( latLoc ) / a, 2 ) + pow( sin( latLoc ) / b, 2 ) );

    //distances North of the local latitude can be approximated by the local
    //radius minus the depth, multiplied by the sine of the angle between the
    //latitudes and the local latitude
    Ym = ( r - depLoc ) * sin( lat - latLoc );

    //distances East of the local longitude can be approximated by the local
    //radius minus the depth, multiplied by the sine of the angle between the
    //longitudes and the local longitude, and scaled by the cosine of the local
    //latitude
    Xm  = ( r - depLoc ) * sin( lon - lonLoc ) * cos( latLoc );
}

void InternalEnvSim::transverseMercatorToCoordinates( double &Ym, double &Xm, double depLoc, double latDeg, double lonDeg )
{
    TransverseMercatorToCoordinates( Ym, Xm, depLoc, latDeg, lonDeg,
                                     latProjectionOrigin_, lonCentralMeridian_, falseEasting_, falseNorthing_, scaleCentralMeridian_ );
}

void InternalEnvSim::TransverseMercatorToCoordinates( double &Ym, double &Xm, double depLoc, double latDeg, double lonDeg,
        double latProjectionOrigin, double lonCentralMeridian, double falseEasting, double falseNorthing, double scaleCentralMeridian )
{

    double lat = D2R( latDeg );
    double lon = D2R( lonDeg );
    double originLatitude = D2R( latProjectionOrigin );
    double centralMeridian = D2R( lonCentralMeridian );

    long error = Wgs84::LatLonToTransverseMercator( lat, lon, Ym, Xm, originLatitude, centralMeridian,
                 falseEasting, falseNorthing, scaleCentralMeridian );
    if( error != 0 )
    {
        printf( "Error on conversion of position to transverse mercator coordinates: %s\n", Wgs84::TransMercErrorToString( error ) );
    }

}

void InternalEnvSim::simulateSensors( double dTime, double depthM, double latDeg, double lonDeg, float* values )
{
    double latDegOrYm, lonDegOrXm;
    switch( gridMapping_ )
    {
    default:
    case GRID_MAPPING_NONE:
        latDegOrYm = latDeg;
        lonDegOrXm = lonDeg;
        break;

    case GRID_MAPPING_AZIMUTHAL_EQUIDISTANT:
        azimuthalEquidistantToCoordinates( latDegOrYm, lonDegOrXm, depthM, latDeg, lonDeg );
        break;

    case GRID_MAPPING_TRANSVERSE_MERCATOR:
        transverseMercatorToCoordinates( latDegOrYm, lonDegOrXm, depthM, latDeg, lonDeg );
        break;
    }


    int timeIndex( 0 ), depthIndex( 0 ), latOrYIndex( 0 ), lonOrXIndex( 0 );
    float time( dTime );

    if( NULL != netCdf3Reader_ )
    {

        timeIndex = AuvMath::FindLtEqIndex( modelTime_, time, 0, modelTimeSize_ - 1 );
        if( debug_ && timeIndex < 0 ) logger_.syslog( "Time " + Str( time ) + " outside of range " + Str( modelTime_[0] ) + " to " + Str( modelTime_[modelTimeSize_ - 1] ), Syslog::INFO );
        if( depthM < 0 )
        {
            depthM = 0;
        }

        depthIndex = AuvMath::FindLtEqIndex( modelDepth_, ( float )depthM, 0, modelDepthSize_ - 1 );
        if( debug_ && depthIndex < 0 ) logger_.syslog( "Depth " + Str( depthM ) + " outside of range " + Str( modelDepth_[0] ) + " to " + Str( modelDepth_[modelDepthSize_ - 1] ), Syslog::INFO );
        if( modelY_ == NULL )
        {
            latOrYIndex = AuvMath::FindLtEqIndex( modelLatitude_, ( float )latDegOrYm, 0, modelLatitudeSize_ - 1 );
            lonOrXIndex = AuvMath::FindLtEqIndex( modelLongitude_, ( float )lonDegOrXm, 0, modelLongitudeSize_ - 1 );
            if( debug_ && latOrYIndex < 0 ) logger_.syslog( "Lat " + Str( latDegOrYm ) + " outside of range " + Str( modelLatitude_[0] ) + " to " + Str( modelLatitude_[modelLatitudeSize_ - 1] ), Syslog::INFO );
            if( debug_ && lonOrXIndex < 0 ) logger_.syslog( "Lat " + Str( lonDegOrXm ) + " outside of range " + Str( modelLongitude_[0] ) + " to " + Str( modelLongitude_[modelLongitudeSize_ - 1] ), Syslog::INFO );
        }
        else
        {
            latOrYIndex = AuvMath::FindLtEqIndex( modelY_, ( float )latDegOrYm, 0, modelYSize_ - 1 );
            lonOrXIndex = AuvMath::FindLtEqIndex( modelX_, ( float )lonDegOrXm, 0, modelXSize_ - 1 );
            if( debug_ && latOrYIndex < 0 ) logger_.syslog( "Y " + Str( latDegOrYm ) + " outside of range " + Str( modelY_[0] ) + " to " + Str( modelY_[modelYSize_ - 1] ), Syslog::INFO );
            if( debug_ && lonOrXIndex < 0 ) logger_.syslog( "X " + Str( lonDegOrXm ) + " outside of range " + Str( modelX_[0] ) + " to " + Str( modelX_[modelXSize_ - 1] ), Syslog::INFO );
        }
    }

    for( unsigned int i = 0; i < varNameCfgSettings_.size(); ++i )
    {
        if( vars_[i].var_ != NULL )
        {
            if( debug_ )printf( "Interpolating %s\n", varNameCfgSettings_[i]->cStr() );
            float value( nanf( "" ) );
            interpolate( time, depthM, latDegOrYm, lonDegOrXm, timeIndex, depthIndex, latOrYIndex, lonOrXIndex, vars_[i], value );
            values[i] = value;
        }
        else
        {
            logger_.syslog( "No variable associated with " + *varNameCfgSettings_[i] );
        }
    }
}

NetCdf3::NetCdfAtt* InternalEnvSim::findAtt( NetCdf3Reader* nc3Reader, const Str& name )
{
    NetCdf3::NetCdfAtt* att = cfgAtts_.get( name );
    if( NULL != att )
    {
        return att;
    }
    return nc3Reader->findAtt( name );
}

bool InternalEnvSim::interpolate( const float& time, const float& depth, const float& lat, const float& lon,
                                  int& timeIndex, int& depthIndex, int& latIndex, int& lonIndex,
                                  VarData& var, float& value )
{
    return interpolate( time, depth, lat, lon, timeIndex, depthIndex, latIndex, lonIndex, var.indices_, var.array_, var.var_, value );
}

bool InternalEnvSim::interpolate( const float& time, const float& depth, const float& lat, const float& lon,
                                  int& timeIndex, int& depthIndex, int& latIndex, int& lonIndex,
                                  int varIndices[4], float varArray[2][2][2][2], NetCdf3Reader::NetCdfVar* var, float& value )
{
    if( debug_ ) printf( "interpolate( time=%g, depth=%g, latDegOrYm=%g, lonDegOrXm=%g, timeIndex=%d, depthIndex=%d, latOrYIndex=%d, lonOrXIndex=%d)\n", time, depth, lat, lon, timeIndex, depthIndex, latIndex, lonIndex );
    if( depthIndex < 0 || latIndex < 0 || lonIndex < 0 )
    {
        if( debug_ ) printf( "Not interpolating -- out of range\n" );
    }
    else
    {
        if( ( int )modelTimeSize_ - 1 == timeIndex )
        {
            timeIndex = modelTimeSize_ - 2;
        }
        if( timeIndex < 0 )
        {
            timeIndex = 0;
        }
        if( ( int )modelDepthSize_ - 1 == depthIndex )
        {
            depthIndex = modelDepthSize_ - 2;
        }
        if( depthIndex < 0 )
        {
            depthIndex = 0;
        }
        if( ( int )modelLatitudeSize_ - 1 == latIndex )
        {
            latIndex = modelLatitudeSize_ - 2;
        }
        if( latIndex < 0 )
        {
            latIndex = 0;
        }
        if( ( int )modelLongitudeSize_ - 1 == lonIndex )
        {
            lonIndex = modelLongitudeSize_ - 2;
        }
        if( lonIndex < 0 )
        {
            lonIndex = 0;
        }
        if( timeIndex != varIndices[0]
                || depthIndex != varIndices[1]
                || latIndex != varIndices[2]
                || lonIndex != varIndices[3] )
        {
            varIndices[0] = timeIndex;
            varIndices[1] = depthIndex;
            varIndices[2] = latIndex;
            varIndices[3] = lonIndex;
            //printf( "depthIndex=%d, latIndex=%d, lonIndex=%d\n", depthIndex, latIndex, lonIndex );


            if( NULL == decompress_ )
            {
                if( debug_ )
                {
                    printf( "Calling netCdf3Reader_(%08ZX)->read4DArray( %08ZX, %d, %08ZX, %d, %d, %d, %d, %d, %d, %d, %d)\n",
                            ( size_t )netCdf3Reader_, ( size_t )varArray, ( int )NetCdf3Reader::NC_FLOAT, ( size_t )var,
                            timeIndex, timeIndex + ( timeIndex + 1 < ( int )modelTimeSize_ ? 1 : 0 ),
                            depthIndex, depthIndex + 1,
                            latIndex, latIndex + 1,
                            lonIndex, lonIndex + 1 );
                }
                netCdf3Reader_->read4DArray( varArray, NetCdf3Reader::NC_FLOAT, *var,
                                             timeIndex, timeIndex + ( timeIndex + 1 < ( int )modelTimeSize_ ? 1 : 0 ),
                                             depthIndex, depthIndex + 1,
                                             latIndex, latIndex + 1,
                                             lonIndex, lonIndex + 1 );
                if( debug_ )
                {
                    for( int i = 0; i < 2; ++i )
                    {
                        for( int j = 0; j < 2; ++j )
                        {
                            for( int k = 0; k < 2; ++k )
                            {
                                for( int l = 0; l < 2; ++l )
                                {
                                    printf( "varArray[%d][%d][%d][%d]=%g\n", i, j, k, l, varArray[i][j][k][l] );
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                for( int i = 0; i < 2; ++i )
                {
                    for( int j = 0; j < 2; ++j )
                    {
                        int index = ( lonIndex + i ) * modelYSize_ + latIndex + j;
                        int compressedIndex = decompress_[index];
                        if( debug_ )
                        {
                            printf( "In interpolate, index=%d, compressedIndex=%d\n", index, compressedIndex );
                            fflush( stdout );
                        }
                        if( compressedIndex >= 0 )
                        {
                            netCdf3Reader_->read3DArray( varArray[i][j], NetCdf3Reader::NC_FLOAT, *var,
                                                         timeIndex, timeIndex + ( timeIndex + 1 < ( int )modelTimeSize_ ? 1 : 0 ),
                                                         depthIndex, depthIndex + 1,
                                                         compressedIndex, compressedIndex );
                            if( debug_ )
                            {
                                for( int k = 0; k < 2; ++k )
                                {
                                    for( int l = 0; l < 2; ++l )
                                    {
                                        printf( "varArray[%d][%d][%d][%d]=%g\n", i, j, k, l, varArray[i][j][k][l] );
                                    }
                                }
                            }
                        }
                        else
                        {
                            for( int k = 0; k < 2; ++k )
                            {
                                for( int l = 0; l < 2; ++l )
                                {
                                    varArray[i][j][k][l] = nan( "" );
                                }
                            }
                        }
                    }
                }
            }


            // a little cheesy clean-up for ROMS model...
            for( int i = 0; i < 1 + ( timeIndex + 1 < ( int )modelTimeSize_ ? 1 : 0 ); ++i )
            {
                for( int j = 0; j < 2; ++j )
                {
                    for( int k = 0; k < 2; ++k )
                    {
                        for( int l = 0; l < 2; ++l )
                        {
                            if( -9999.0f == varArray[i][j][k][l] )
                            {
                                varArray[i][j][k][l] = 0.0f;
                            }
                        }
                    }
                }
            }
        }

        // TODO replace static ROMS model outptut withtime-varying field

        if( modelTimeSize_ == 1 )
        {
            value = AuvMath::Interpolate3D( depth, lat, lon, varArray[0],
                                            modelDepth_[ depthIndex ], modelDepth_[ depthIndex + 1 ],
                                            modelLatitude_[ latIndex ], modelLatitude_[ latIndex + 1 ],
                                            modelLongitude_[ lonIndex ], modelLongitude_[ lonIndex + 1 ] );
        }
        else
        {
            if( modelXSize_ == 0 )
            {
                value = AuvMath::Interpolate4D( time, depth, lat, lon, varArray,
                                                modelTime_[ timeIndex ], modelTime_[ timeIndex + 1 ],
                                                modelDepth_[ depthIndex ], modelDepth_[ depthIndex + 1 ],
                                                modelLatitude_[ latIndex ], modelLatitude_[ latIndex + 1 ],
                                                modelLongitude_[ lonIndex ], modelLongitude_[ lonIndex + 1 ] );
            }
            else
            {
                value = AuvMath::Interpolate4D( time, depth, lat, lon, varArray,
                                                modelTime_[ timeIndex ], modelTime_[ timeIndex + 1 ],
                                                modelDepth_[ depthIndex ], modelDepth_[ depthIndex + 1 ],
                                                modelY_[ latIndex ], modelY_[ latIndex + 1 ],
                                                modelX_[ lonIndex ], modelX_[ lonIndex + 1 ] );
                if( debug_ )
                {
                    printf( "Interpolate4D( %g,%g,%g,%g,[", time, depth, lat, lon );
                    for( int i = 0; i < 2; ++i )
                    {
                        printf( "%c[", i == 0 ? ' ' : ',' );
                        for( int j = 0; j < 2; ++j )
                        {
                            printf( "%c[", j == 0 ? ' ' : ',' );
                            for( int k = 0; k < 2; ++k )
                            {
                                printf( "%c[", k == 0 ? ' ' : ',' );
                                for( int l = 0; l < 2; ++l )
                                {
                                    printf( "%c%g", l == 0 ? ' ' : ',', varArray[i][j][k][l] );
                                }
                                printf( "]" );
                            }
                            printf( "]" );
                        }
                        printf( "]" );
                    }
                    printf( "],%g,%g,%g,%g,%g,%g,%g,%g=%g\n", modelTime_[ timeIndex ], modelTime_[ timeIndex + 1 ],
                            modelDepth_[ depthIndex ], modelDepth_[ depthIndex + 1 ],
                            modelY_[ latIndex ], modelY_[ latIndex + 1 ],
                            modelX_[ lonIndex ], modelX_[ lonIndex + 1 ], value );
                }
            }
        }

        return true;
    }
    return false;
}

float InternalEnvSim::twoLineEstimate( const float x, const float y0, const float y1, const float y2,
                                       const float x0, const float x1, const float x2 )
{
    if( x < x1 )
    {
        if( x < x0 )
        {
            return y0;
        }
        return AuvMath::Interpolate1D( x, y0, y1, x0, x1 );
    }
    else
    {
        if( x > x2 )
        {
            return y2;
        }
        return AuvMath::Interpolate1D( x, y1, y2, x1, x2 );
    }
}