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

#include "EnvSimulator.h"

#include "utils/Datum.h"

EnvSimulator::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;
                }
            }
        }
    }
}


EnvSimulator::EnvSimulator( bool simulateSensors, int nSteps, bool debug )
    : Simulator( simulateSensors, nSteps, debug ),
      netCdfReader_( NULL ),                 // Gridded data file reader
      modelTime_( NULL ),                    // time array for netCdfReader
      modelTimeSize_( 0 ),                   // size of time Array
      modelDepth_( NULL ),                   // depth array for netCdfReader
      modelDepthSize_( 0 ),                  // size of depth Array
      modelLatitude_( NULL ),                // latitude array for netCdfReader
      modelLatitudeSize_( 0 ),               // size of latitude Array
      modelLongitude_( NULL ),               // longitude array for netCdfReader
      modelLongitudeSize_( 0 ),              // size of longitude Array
      varLatitude_( NULL ),
      varLongitude_( NULL ),
      modelY_( NULL ),                       // projection Y array for netCdfReader
      modelYSize_( 0 ),                      // size of projection Y Array
      modelX_( NULL ),                       // projection X array for netCdfReader
      modelXSize_( 0 ),                      // size of projection X Array
      varSalinity_(),                        // Variable that contains salinity
      varTemperature_(),                     // Variable that contains temperature
      varEastCurrent_(),                     // Variable that contains u
      varNorthCurrent_(),                    // Variable that contains v
      vars_( NULL ),                         // Other 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
{}

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

void EnvSimulator::configureSensors()
{
    netCdfReader_ = NetCdfReader::NewNetCdfReader( oceanModelData_.cStr() );
    if( NULL == netCdfReader_ )
    {
        oceanModelData_ = "../" + oceanModelData_;
        netCdfReader_ = NetCdfReader::NewNetCdfReader( oceanModelData_.cStr() );
    }
    if( NULL != netCdfReader_ )
    {
        //logger_.syslog( "Opened ", oceanModelData_ );
        printf( "Opened %s\n", oceanModelData_.cStr() );
        const NetCdfReader::NetCdfDimArray& netCdfDimArray = netCdfReader_->getDimArray();
        NetCdfReader::NetCdfVar* varTime( netCdfReader_->findNetCdfVarByAttribute( "standard_name", "time" )/*->findVar( "time" )*/ );
        if( NULL != varTime )
        {
            modelTimeSize_ = ( netCdfDimArray.get( varTime->dimIds_[ 0 ] ) )->dimSize_;
            modelTime_ = new float[ modelTimeSize_ ];
            netCdfReader_->read1DArray( modelTime_, NetCdfReader::NC_FLOAT_TYPE, *varTime, 0, modelTimeSize_ - 1 );
        }
        NetCdfReader::NetCdfVar* varDepth = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "depth" );//->findVar( "depth" );
        if( NULL != varDepth )
        {
            modelDepthSize_ = ( netCdfDimArray.get( varDepth->dimIds_[ 0 ] ) )->dimSize_;
            modelDepth_ = new float[ modelDepthSize_ ];
            netCdfReader_->read1DArray( modelDepth_, NetCdfReader::NC_FLOAT_TYPE, *varDepth, 0, modelDepthSize_ - 1 );
        }
        varLongitude_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "longitude" );//->findVar( "lon" );
        if( NULL != varLongitude_ )
        {
            modelLongitudeSize_ = ( netCdfDimArray.get( varLongitude_->dimIds_[ 0 ] ) )->dimSize_;
            modelLongitude_ = new float[ modelLongitudeSize_ ];
            netCdfReader_->read1DArray( modelLongitude_, NetCdfReader::NC_FLOAT_TYPE, *varLongitude_, 0, modelLongitudeSize_ - 1 );
        }
        varLatitude_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "latitude" );//->findVar( "lat" );
        if( NULL != varLatitude_ )
        {
            modelLatitudeSize_ = netCdfDimArray.get( varLatitude_->dimIds_[ 0 ] )->dimSize_;
            modelLatitude_ = new float[ modelLatitudeSize_ ];
            netCdfReader_->read1DArray( modelLatitude_, NetCdfReader::NC_FLOAT_TYPE, *varLatitude_, 0, modelLatitudeSize_ - 1 );
        }
        NetCdfReader::NetCdfVar* varY = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "projection_y_coordinate" );
        NetCdfReader::NetCdfVar* varX = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "projection_x_coordinate" );
        if( NULL != varX && NULL != varY )
        {
            NetCdfReader::NetCdfAtt* attGridMapping = netCdfReader_->findAtt( "grid_mapping_name" );
            if( NULL != attGridMapping )
            {
                Str gridMapping;
                if( netCdfReader_->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_ = ( netCdfDimArray.get( varX->dimIds_[ 0 ] ) )->dimSize_;
                modelX_ = new float[ modelXSize_ ];
                netCdfReader_->read1DArray( modelX_, NetCdfReader::NC_FLOAT_TYPE, *varX, 0, modelXSize_ - 1 );

                modelYSize_ = netCdfDimArray.get( varY->dimIds_[ 0 ] )->dimSize_;
                modelY_ = new float[ modelYSize_ ];
                netCdfReader_->read1DArray( modelY_, NetCdfReader::NC_FLOAT_TYPE, *varY, 0, modelYSize_ - 1 );

                NetCdfReader::NetCdfVar* varCompressYX = netCdfReader_->findNetCdfVarByAttribute( "compress", "y x" );
                if( NULL != varCompressYX )
                {
                    int compressSize = netCdfDimArray.get( varCompressYX->dimIds_[ 0 ] )->dimSize_;
                    int* compress = new int[ compressSize ];
                    netCdfReader_->read1DArray( compress, NetCdfReader::NC_INT_TYPE, *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;
                }
            }
            NetCdfReader::NetCdfAtt* attScaleCentralMeridian = netCdfReader_->findAtt( "scale_factor_at_central_meridian" );
            if( NULL != attScaleCentralMeridian )
            {
                netCdfReader_->readAtt( &scaleCentralMeridian_, NetCdf::NC_DOUBLE_TYPE, attScaleCentralMeridian );
            }
            NetCdfReader::NetCdfAtt* attLonCentralMeridian = netCdfReader_->findAtt( "longitude_of_central_meridian" );
            if( NULL != attLonCentralMeridian )
            {
                netCdfReader_->readAtt( &lonCentralMeridian_, NetCdf::NC_DOUBLE_TYPE, attLonCentralMeridian );
            }
            NetCdfReader::NetCdfAtt* attLonProjectionOrigin = netCdfReader_->findAtt( "longitude_of_projection_origin" );
            if( NULL != attLonProjectionOrigin )
            {
                netCdfReader_->readAtt( &lonProjectionOrigin_, NetCdf::NC_DOUBLE_TYPE, attLonProjectionOrigin );
            }
            NetCdfReader::NetCdfAtt* attLatProjectionOrigin = netCdfReader_->findAtt( "latitude_of_projection_origin" );
            if( NULL != attLatProjectionOrigin )
            {
                netCdfReader_->readAtt( &latProjectionOrigin_, NetCdf::NC_DOUBLE_TYPE, attLatProjectionOrigin );
            }
            NetCdfReader::NetCdfAtt* attFalseEasting = netCdfReader_->findAtt( "false_easting" );
            if( NULL != attFalseEasting )
            {
                netCdfReader_->readAtt( &falseEasting_, NetCdf::NC_DOUBLE_TYPE, attFalseEasting );
            }
            NetCdfReader::NetCdfAtt* attFalseNorthing = netCdfReader_->findAtt( "false_northing" );
            if( NULL != attFalseNorthing )
            {
                netCdfReader_->readAtt( &falseNorthing_, NetCdf::NC_DOUBLE_TYPE, attFalseNorthing );
            }
        }
        else
        {
            gridMapping_ = GRID_MAPPING_NONE;
        }
        if( debug_ )
        {
            printf( "gridMapping=%d\n", gridMapping_ );
        }
        varSalinity_.var_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "sea_water_salinity" );//findVar( "salt" );
        varTemperature_.var_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "sea_water_temperature" );
        varEastCurrent_.var_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "eastward_sea_water_velocity" );
        varNorthCurrent_.var_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", "northward_sea_water_velocity" );
        if( oceanModelVarCount_ > 0 )
        {
            vars_ = new VarData[oceanModelVarCount_];
            for( int i = 0; i < oceanModelVarCount_; ++i )
            {
                vars_[i].var_ = netCdfReader_->findNetCdfVarByAttribute( "standard_name", oceanModelVarNames_[i] );
                if( debug_ )printf( "Found %s at %08ZX\n", oceanModelVarNames_[i].cStr(), ( size_t )vars_[i].var_ );
            }
        }
    }
    else
    {
        printf( "Error -- could not open netCDF file at %s!\n", oceanModelData_.cStr() );
    }
}

void EnvSimulator::azimuthalEquidistantToCoordinates( double& Ym, double& Xm, double depLoc, double latDeg, double lonDeg )
{
    //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 EnvSimulator::transverseMercatorToCoordinates( double& Ym, double& Xm, double depLoc, double latDeg, double lonDeg )
{

    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 EnvSimulator::simulateSensors( double dTime, double latDeg, double lonDeg, SimResultStruct& results )
{
    float depth( pos_.getZ() );
    double latDegOrYm, lonDegOrXm;
    switch( gridMapping_ )
    {
    default:
    case GRID_MAPPING_NONE:
        latDegOrYm = latDeg;
        lonDegOrXm = lonDeg;
        break;

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

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


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

    if( simulateSensors_ && NULL != netCdfReader_ )
    {

        timeIndex = AuvMath::FindLtEqIndex( modelTime_, time, 0, modelTimeSize_ - 1 );
        if( depth < 0 )
        {
            depth = 0;
        }
        depthIndex = AuvMath::FindLtEqIndex( modelDepth_, depth, 0, modelDepthSize_ - 1 );
        if( modelY_ == NULL )
        {
            latOrYIndex = AuvMath::FindLtEqIndex( modelLatitude_, ( float )latDegOrYm, 0, modelLatitudeSize_ - 1 );
            lonOrXIndex = AuvMath::FindLtEqIndex( modelLongitude_, ( float )lonDegOrXm, 0, modelLongitudeSize_ - 1 );
        }
        else
        {
            latOrYIndex = AuvMath::FindLtEqIndex( modelY_, ( float )latDegOrYm, 0, modelYSize_ - 1 );
            lonOrXIndex = AuvMath::FindLtEqIndex( modelX_, ( float )lonDegOrXm, 0, modelXSize_ - 1 );
        }
    }

    if( simulateSensors_ && NULL != netCdfReader_ )
    {
        float eastCurent;
        float northCurrent;
        if( debug_ ) printf( "Interpolate eastCurrent \n" );
        bool interpolatedEast = varEastCurrent_.var_ && interpolate( time, depth, latDegOrYm, lonDegOrXm, timeIndex, depthIndex, latOrYIndex, lonOrXIndex, varEastCurrent_, eastCurent );
        if( debug_ ) printf( "Interpolate northCurrent \n" );
        bool interpolatedNorth = varNorthCurrent_.var_ && interpolate( time, depth, latDegOrYm, lonDegOrXm, timeIndex, depthIndex, latOrYIndex, lonOrXIndex, varNorthCurrent_,  northCurrent );
        if( interpolatedEast && interpolatedNorth && eastCurent == eastCurent && northCurrent == northCurrent )
        {
            // This is not an error.
            // The sim definition is U = northward, V = eastward.
            current_.setU( northCurrent );
            current_.setV( eastCurent );
            if( debug_ )
            {
                printf( "%s\n", current_.toString().cStr() );
            }
        }
        else if( varEastCurrent_.var_ && varNorthCurrent_.var_ && debug_ )
        {
            printf( "Current interpolation failed, interpolatedEast=%d, interpolatedNorth=%d, eastCurent=%g, northCurrent=%g\n", interpolatedEast, interpolatedNorth, eastCurent, northCurrent );
        }
    }

    if( vertCurrent_ == vertCurrent_ )
    {
        current_.setW( vertCurrent_ );
    }

    results.magneticVariation_ = magneticVariation_;
    results.soundSpeed_ = soundSpeed_;

    float temperature( nan( "" ) );
    if( sst_ == sst_ && tMixed_ == tMixed_ && t300_ == t300_ && mixedLayerDepth_ == mixedLayerDepth_ )
    {
        temperature = twoLineEstimate( depth, sst_, tMixed_, t300_, 0.0, mixedLayerDepth_, 300.0 );
    }
    else if( simulateSensors_ && NULL != netCdfReader_ && NULL != varTemperature_.var_ )
    {
        if( debug_ ) printf( "Interpolate temperature \n" );
        interpolate( time, depth, latDegOrYm, lonDegOrXm, timeIndex, depthIndex, latOrYIndex, lonOrXIndex, varTemperature_, temperature );
    }
    results.temperature_ = temperature;

    float salinity( nan( "" ) );
    if( sss_ == sss_ && sMixed_ == sMixed_ && s300_ == s300_ && mixedLayerDepth_ == mixedLayerDepth_ )
    {
        salinity = twoLineEstimate( depth, sss_, sMixed_, s300_, 0.0, mixedLayerDepth_, 300.0 );
    }
    else if( simulateSensors_ && NULL != netCdfReader_ && NULL != varSalinity_.var_ )
    {
        if( debug_ ) printf( "Interpolate salinity \n" );
        interpolate( time, depth, latDegOrYm, lonDegOrXm, timeIndex, depthIndex, latOrYIndex, lonOrXIndex, varSalinity_, salinity );
    }
    results.salinity_ = salinity;

    if( density_ == density_ )
    {
        lastDensity_ = density_;
    }
    else if( salinity == salinity && temperature == temperature )
    {
        float pressure = AuvMath::OceanPressure( depth, latDeg );
        lastDensity_ = AuvMath::Density( salinity, temperature + 273.15, pressure );
    }
    results.density_ = density_;

    for( int i = 0; i < oceanModelVarCount_; ++i )
    {
        if( vars_[i].var_ != NULL )
        {
            if( debug_ )printf( "Interpolating %s\n", oceanModelVarNames_[i].cStr() );
            float value( nanf( "" ) );
            interpolate( time, depth, latDegOrYm, lonDegOrXm, timeIndex, depthIndex, latOrYIndex, lonOrXIndex, vars_[i], value );
            results.values_[i] = value;
        }
    }
}

bool EnvSimulator::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 EnvSimulator::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], NetCdfReader::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 netCdfReader_(%08ZX)->read4DArray( %08ZX, %d, %08ZX, %d, %d, %d, %d, %d, %d, %d, %d)\n",
                            ( size_t )netCdfReader_, ( size_t )varArray, ( int )NetCdfReader::NC_FLOAT_TYPE, ( size_t )var,
                            timeIndex, timeIndex + ( timeIndex + 1 < ( int )modelTimeSize_ ? 1 : 0 ),
                            depthIndex, depthIndex + 1,
                            latIndex, latIndex + 1,
                            lonIndex, lonIndex + 1 );
                }
                netCdfReader_->read4DArray( varArray, NetCdfReader::NC_FLOAT_TYPE, *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 )
                        {
                            netCdfReader_->read3DArray( varArray[i][j], NetCdfReader::NC_FLOAT_TYPE, *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 EnvSimulator::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 );
    }
}