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

#include "HFRadarModelPoint.h"
#include "HFRadarModelPointIF.h"

#include "controlModule/HorizontalControlIF.h"
#include "data/Location.h"
#include "data/UniversalDataReader.h"
#include "estimationModule/HFRadarModelCalc.h"
#include "estimationModule/LocationNode.h"
#include "utils/AuvMath.h"

HFRadarModelPoint::HFRadarModelPoint( const Str& prefix, const Module* module )
    : Behavior( prefix + HFRadarModelPointIF::NAME, module, true, true ),
      latitudeSetting_( nan( "" ) ),
      longitudeSetting_( nan( "" ) ),
      speedSetting_( nan( "" ) ),
      intermediateLatitude_( nan( "" ) ),
      intermediateLongitude_( nan( "" ) ),
      subpoints_( true ),
      needSubpoints_( false ),
      calculatingSubpoints_( false ),
      refiningSubpoints_( false ),
      fineLatSize_( 0 ),
      fineLat_( NULL ),
      fineLonSize_( 0 ),
      fineLon_( NULL ),
      fineU_( NULL ),
      fineV_( NULL ),
      bearing_( nan( "" ) ),
      finalPerpendicularSlope_( nan( "" ) ),
      finalStartOverLine_( false ),
      intermediatePerpendicularSlope_( nan( "" ) ),
      intermediateStartOverLine_( false ),
      initialized_( false ),
      initTime_( 0 ),
      timeSize_( 0 ),
      calcCompleteTime_( Timestamp::NOT_SET_TIME ),
      startNode_( NULL ),
      goalNode_( NULL ),
      nodeMap_( true ),
      nodeQueue_( true, 128 ),
      minNodeCount_( 0 )
{
    logger_.syslog( "Construct HFRadarModelPoint." );

    // Slate input setting variables
    distanceDeltaSettingReader_ = newSettingReader( HFRadarModelPointIF::DISTANCE_DELTA_SETTING );
    distanceDeltaBearingSettingReader_ = newSettingReader( HFRadarModelPointIF::DISTANCE_DELTA_BEARING_SETTING );
    eastingsDeltaSettingReader_ = newSettingReader( HFRadarModelPointIF::EASTINGS_DELTA_SETTING );
    latitudeSettingReader_ = newSettingReader( HFRadarModelPointIF::LATITUDE_SETTING );
    latitudeDeltaSettingReader_ = newSettingReader( HFRadarModelPointIF::LATITUDE_DELTA_SETTING );
    longitudeSettingReader_ = newSettingReader( HFRadarModelPointIF::LONGITUDE_SETTING );
    longitudeDeltaSettingReader_ = newSettingReader( HFRadarModelPointIF::LONGITUDE_DELTA_SETTING );
    northingsDeltaSettingReader_ = newSettingReader( HFRadarModelPointIF::NORTHINGS_DELTA_SETTING );
    speedSettingReader_ = newSettingReader( HFRadarModelPointIF::SPEED_SETTING );
    gridDivideSettingReader_ = newSettingReader( HFRadarModelPointIF::GRID_DIVIDE );

    // Slate input measurements
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

    // Slate output variables
    bearingCmdWriter_ = newDataWriter( HorizontalControlIF::BEARING_CMD );
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    latitudeCmdWriter_ = newDataWriter( HorizontalControlIF::LATITUDE_CMD );
    longitudeCmdWriter_ = newDataWriter( HorizontalControlIF::LONGITUDE_CMD );

    this->setFailureMissionCritical( true ); // explicitly set mission critical so that it exits the mission if it cannot find a path

}

HFRadarModelPoint::~HFRadarModelPoint()
{
    if( NULL != startNode_ )
    {
        delete startNode_;
    }
    if( NULL != goalNode_ )
    {
        delete goalNode_;
    }
    clearFine();
}

void HFRadarModelPoint::calcCompletion( double startLatitude,
                                        double startLongitude, double latitudeGoal, double longitudeGoal,
                                        double& perpendicularSlope, bool& startOverLine )
{

    /// Slope of the line perpendicular between now and the waypoint
    if( latitudeGoal != startLatitude )
    {
        perpendicularSlope = -( longitudeGoal - startLongitude )
                             / ( latitudeGoal - startLatitude );
        /// Is our starting point latitude > than the latitude of the starting
        /// longitude on the crossing line?
        startOverLine = startLatitude
                        > latitudeGoal
                        + ( startLongitude - longitudeGoal )
                        * perpendicularSlope;
    }
    else
    {
        perpendicularSlope = nan( "" );
        /// In this case, is the starting point longitude > than the setting longitude?
        startOverLine = startLongitude > longitudeGoal;
    }
}

void HFRadarModelPoint::initialize( void )
{
    logger_.syslog( "Initialize HFRadarModelPoint." );

    if( !speedSettingReader_->read( Units::METER_PER_SECOND, speedSetting_ )
            || isnan( speedSetting_ ) )
    {
        logger_.syslog( "No valid Speed setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;;
    }


    if( !latitudeSettingReader_->isActive()
            && !longitudeSettingReader_->isActive()
            && !latitudeDeltaSettingReader_->isActive()
            && !longitudeDeltaSettingReader_->isActive()
            && !eastingsDeltaSettingReader_->isActive()
            && !northingsDeltaSettingReader_->isActive()
            && !distanceDeltaSettingReader_->isActive()
            && !distanceDeltaBearingSettingReader_->isActive() )
    {
        logger_.syslog( "No Waypoint setting.", Syslog::CRITICAL );
        initialized_ = false;
        return;
    }

    double startLatitude;
    double startLongitude;
    if( !latitudeReader_->isActive()
            || !latitudeReader_->read( Units::RADIAN, startLatitude )
            || isnan( startLatitude )
            || !longitudeReader_->isActive()
            || !longitudeReader_->read( Units::RADIAN, startLongitude )
            || isnan( startLongitude ) )
    {
        printf( "************** WP NOT INITIALIZED! \n" );
        return;
    }

    // Configure settingLatitude_
    if( !latitudeSettingReader_->isActive()
            || !latitudeSettingReader_->read( Units::RADIAN, latitudeSetting_ )
            || isnan( latitudeSetting_ ) )
    {
        latitudeSetting_ = startLatitude;
    }
    float latitudeDelta;
    if( latitudeDeltaSettingReader_->isActive()
            &&	latitudeDeltaSettingReader_->read( Units::RADIAN, latitudeDelta )
            && !isnan( latitudeDelta ) )
    {
        latitudeSetting_ += latitudeDelta;
    }

    float northingsDeltaSetting( nan( "" ) );
    if( northingsDeltaSettingReader_->read( Units::METER, northingsDeltaSetting )
            && !isnan( northingsDeltaSetting ) )
    {
        latitudeSetting_ = Location::NorthingsDelta( latitudeSetting_, northingsDeltaSetting );
    }

    // Configure settingLongitude_
    if( ! longitudeSettingReader_->isActive()
            || !longitudeSettingReader_->read( Units::RADIAN, longitudeSetting_ )
            || isnan( longitudeSetting_ ) )
    {
        longitudeSetting_ = startLongitude;
    }
    float longitudeDelta;
    if( longitudeDeltaSettingReader_->isActive()
            &&	longitudeDeltaSettingReader_->read( Units::RADIAN, longitudeDelta )
            && !isnan( longitudeDelta ) )
    {
        longitudeSetting_ += longitudeDelta;
    }
    float eastingsDeltaSetting( nan( "" ) );
    if( eastingsDeltaSettingReader_->read( Units::METER, eastingsDeltaSetting )
            && !isnan( eastingsDeltaSetting ) )
    {
        longitudeSetting_ = Location::EastingsDelta( latitudeSetting_, longitudeSetting_, eastingsDeltaSetting );
    }

    float distanceDeltaSetting( nanf( "" ) );
    if( distanceDeltaSettingReader_->isActive()
            && distanceDeltaSettingReader_->read( Units::METER, distanceDeltaSetting )
            && !isnan( distanceDeltaSetting ) )
    {
        float distanceDeltaBearingSetting( nanf( "" ) );
        if( !distanceDeltaBearingSettingReader_->isActive()
                || !distanceDeltaBearingSettingReader_->read( Units::RADIAN, distanceDeltaBearingSetting )
                || isnan( distanceDeltaBearingSetting ) )
        {
            logger_.syslog( "Must specify both distanceDelta and distanceDeltaBearing, or neither", Syslog::CRITICAL );
        }
        Location::AtBearing( distanceDeltaBearingSetting, distanceDeltaSetting, latitudeSetting_, longitudeSetting_ );
    }

    /// Slope of the line perpendicular between now and the waypoint
    calcCompletion( startLatitude, startLongitude, latitudeSetting_,
                    longitudeSetting_, finalPerpendicularSlope_, finalStartOverLine_ );

    intermediateLatitude_ = latitudeSetting_;
    intermediateLongitude_ = longitudeSetting_;
    intermediatePerpendicularSlope_ = finalPerpendicularSlope_;
    intermediateStartOverLine_ = finalStartOverLine_;

    bearing_ = Location::GetBearing( startLatitude, startLongitude,
                                     latitudeSetting_, longitudeSetting_ );

    latitudeReader_->requestData( true );
    longitudeReader_->requestData( true );

    initialized_ = true;
    initTime_ = Timestamp::Now().asDouble();
    //printf("*********************** initTime_=%g\n",initTime_);
}

bool HFRadarModelPoint::isComplete( double latitude, double longitude,
                                    double latitudeGoal, double longitudeGoal, double perpendicularSlope,
                                    bool startOverLine )
{

    bool isOverLine;
    if( !isnan( perpendicularSlope ) )
    {
        isOverLine = latitude
                     > latitudeGoal
                     + ( longitude - longitudeGoal ) * perpendicularSlope;
    }
    else
    {
        isOverLine = longitude > longitudeGoal;
    }
    return isOverLine != startOverLine;

}

bool HFRadarModelPoint::isSatisfied()
{
    if( !initialized_ )
    {
        initialize();
    }
    if( !initialized_ )
    {
        return false;
    }

    if( subpoints_.size() > 0 )
    {
        return false;
    }

    double latitude;
    double longitude;
    latitudeReader_->read( Units::RADIAN, latitude );
    longitudeReader_->read( Units::RADIAN, longitude );

    if( isnan( latitude ) || isnan( longitude ) )
    {
        logger_.syslog( "Location is nan.", Syslog::ERROR );
        return false;
    }

    return isComplete( latitude, longitude, latitudeSetting_, longitudeSetting_,
                       finalPerpendicularSlope_, finalStartOverLine_ );
}

void HFRadarModelPoint::run( void )
{
    if( !initialized_ )
    {
        initialize();
    }
    if( !initialized_ )
    {
        logger_.syslog( "Could not initialize.", Syslog::FAULT );
        this->setFailure( FailureMode::SOFTWARE );
        return;
    }

    double latitude;
    double longitude;
    latitudeReader_->read( Units::RADIAN, latitude );
    longitudeReader_->read( Units::RADIAN, longitude );

    if( isnan( latitude ) || isnan( longitude ) )
    {
        return;
    }

    if( HFRadarModelCalc::Instance()->getCalculationCompleteTime() > calcCompleteTime_ )
    {
        calcCompleteTime_ = HFRadarModelCalc::Instance()->getCalculationCompleteTime();
        needSubpoints_ = true;
    }

    if( needSubpoints_ || calculatingSubpoints_ )
    {
        const double* time = HFRadarModelCalc::Instance()->getTime( timeSize_ );
        int latSize;
        float* lat;
        int lonSize;
        float* lon;
        float*** u;
        float*** v;
        const float& maxCurrent = HFRadarModelCalc::Instance()->getMaxCurrent();

        if( !refiningSubpoints_ || needSubpoints_ )
        {
            lat = HFRadarModelCalc::Instance()->getLat( latSize );
            lon = HFRadarModelCalc::Instance()->getLon( lonSize );
            u = HFRadarModelCalc::Instance()->getU();
            v = HFRadarModelCalc::Instance()->getV();
        }
        else
        {
            latSize = fineLatSize_;
            lat = fineLat_;
            lonSize = fineLonSize_;
            lon = fineLon_;
            u = fineU_;
            v = fineV_;
        }

        if( needSubpoints_ )
        {
            logger_.syslog( "Start calculating subpoints.", Syslog::IMPORTANT );

            if( NULL != startNode_ )
            {
                delete startNode_;
            }
            if( NULL != goalNode_ )
            {
                delete goalNode_;
            }

            goalNode_ = new LocationNode( latitudeSetting_, longitudeSetting_, -1, -1 );
            startNode_ = new LocationNode( latitude, longitude, -1, -1 );
            startNode_->setDScoreAndHeuristic( timeOfRun_.asDouble() - initTime_,
                                               heuristic( startNode_, maxCurrent ) );

            initNodes( latitude, longitude, latSize, lonSize, lat, lon, maxCurrent );

            needSubpoints_ = false;
            calculatingSubpoints_ = true;
            refiningSubpoints_ = false;
        }
        LocationNode* minNode = NULL;
        // Loop for nominally up to 50 msec...
        while( timeOfRun_.elapsed() < 0.05 )
        {
            minNode = nodeQueue_.popIndexed( 0 );
            if( NULL != minNode )
            {
                ++minNodeCount_;
                //printf( "@@@@@@@@@@@@@ minNode at %s, iLat=%d, iLon=%d, elapsed=%g hours\n", minNode->toString( Units::DEGREE ).cStr(), minNode->getLatIndex(), minNode->getLonIndex(), minNode->getDScore() / 3600 );
                if( minNode == goalNode_ )
                {
                    break;
                }
                minNode->setNodeColor( LocationNode::BLACK );
                FlexArray<LocationNode*> adjs( false );
                getAdjoining( minNode, adjs, latSize, lonSize, lat, lon );
                //printf( "adjs.size()=%d\n", adjs.size() );
                while( adjs.size() > 0 )
                {
                    LocationNode* adj = adjs.pop();
                    if( minNode->getDScore() < adj->getDScore() )
                    {
                        //printf( "calculating travel time from %s(%d,%d) to %s(%d,%d)\n",
                        //        minNode->toString( Units::DEGREE ).cStr(), minNode->getLatIndex(), minNode->getLonIndex(),
                        //        adj->toString( Units::DEGREE ).cStr(), adj->getLatIndex(), adj->getLonIndex());
                        float tt = travelTime( minNode, adj, time, timeSize_, u, v );
                        //printf( "travel time is %g s\n", tt );
                        if( isnan( tt ) )
                        {
                            continue;
                        }
                        float newDScore = minNode->getDScore() + tt;
                        if( newDScore < adj->getDScore() )
                        {
                            adj->setCameFrom( minNode, newDScore, heuristic( adj, maxCurrent ), nodeQueue_ );
                        }
                    }
                }
            }
            else
            {
                calculatingSubpoints_ = false;
                logger_.syslog( "Could not find a path after #paths examined=", minNodeCount_, Syslog::FAULT );
                this->setFailure( FailureMode::SOFTWARE );
                /* alternatively, just point toward the end goal
                // Set the waypoint / heading
                bearing_ = Location::GetBearing( latitude_, longitude_, latitudeSetting_, longitudeSetting_ );
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::WAYPOINT );
                bearingCmdWriter_->write( Units::RADIAN, bearing_ );
                latitudeCmdWriter_->write( Units::RADIAN, latitudeSetting_ );
                longitudeCmdWriter_->write( Units::RADIAN, longitudeSetting_ );
                */
                break;
            }
        }
        if( minNode == goalNode_ && !refiningSubpoints_ )
        {
            logger_.syslog( "Finished first pass subpoints.  Now refining.", Syslog::IMPORTANT );
            makeFine( timeSize_, latSize, lonSize, lat, lon, u, v );
            goalNode_->setCameFrom( NULL, INFINITY, INFINITY, nodeQueue_ );
            initNodes( latitude, longitude, fineLatSize_, fineLonSize_, fineLat_, fineLon_, maxCurrent );
            refiningSubpoints_ = true;
        }
        else if( minNode == goalNode_ )
        {
            // Set up the sub-waypoints (subPoints_)
            //printf( "******************* minNode == goalNode_ *******************\n" );
            logger_.syslog( "Finished refining subpoints.  Now following subpoints.", Syslog::IMPORTANT );
            subpoints_.clear();
            LocationNode* node = goalNode_->getCameFrom();
            // skip the last (in reverse order) node
            if( NULL != node )
            {
                node = node->getCameFrom();
            }
            while( NULL != node )
            {
                // skip the first (in reverse order) node
                if( NULL != node->getCameFrom() && NULL != node->getCameFrom()->getCameFrom() )
                {

                    subpoints_.insert( 0, new Location( *node ) );
                }
                node = node->getCameFrom();
            }

            Str kml = Str( "\n<kml><Document><name>HFRadarModelPoint path</name><Placemark><LineString><coordinates>\n" )
                      + R2D( longitude ) + "," + R2D( latitude ) + "\n";
            for( int i = 0; i < ( int )subpoints_.size(); ++i )
            {
                //logger_.syslog( "Subpoint#" + Str( i, 10 ) + ": " + subpoints_[i]->toString( Units::DEGREE ), Syslog::IMPORTANT );
                kml += Str( subpoints_[i]->getLon( Units::DEGREE ) ) + "," + subpoints_[i]->getLat( Units::DEGREE ) + "\n";
            }
            kml += Str( R2D( longitudeSetting_ ) ) + "," + R2D( latitudeSetting_ )
                   + "\n</coordinates></LineString></Placemark></Document></kml>\n";
            logger_.syslog( kml, Syslog::IMPORTANT );

            calculatingSubpoints_ = false;
            refiningSubpoints_ = false;
            if( subpoints_.size() > 0 )
            {
                Location* loc = subpoints_.get( 0 );
                intermediateLatitude_ = loc->getLat( Units::RADIAN );
                intermediateLongitude_ = loc->getLon( Units::RADIAN );
                calcCompletion( latitude, longitude, intermediateLatitude_, intermediateLongitude_,
                                intermediatePerpendicularSlope_, intermediateStartOverLine_ );
                bearing_ = Location::GetBearing( latitude, longitude,
                                                 intermediateLatitude_, intermediateLongitude_ );
                calcCompletion( intermediateLatitude_, intermediateLongitude_, latitudeSetting_,
                                longitudeSetting_, finalPerpendicularSlope_, finalStartOverLine_ );
            }
            else
            {
                intermediateLatitude_ = latitudeSetting_;
                intermediateLongitude_ = longitudeSetting_;
            }
        }
    }

    // Normal run stuff
    if( subpoints_.size() > 0 && !calculatingSubpoints_ )
    {
        if( isComplete( latitude, longitude, intermediateLatitude_, intermediateLongitude_,
                        intermediatePerpendicularSlope_, intermediateStartOverLine_ ) )
        {
            logger_.syslog( "Reached intermediate subpoint: " + Str( R2D( intermediateLatitude_ ) ) + "," + Str( R2D( intermediateLongitude_ ) ), Syslog::IMPORTANT );
            delete subpoints_.pop( 0 );
            if( subpoints_.size() > 0 )
            {
                Location* loc = subpoints_.get( 0 );
                intermediateLatitude_ = loc->getLat( Units::RADIAN );
                intermediateLongitude_ = loc->getLon( Units::RADIAN );
                calcCompletion( latitude, longitude, intermediateLatitude_, intermediateLongitude_,
                                intermediatePerpendicularSlope_, intermediateStartOverLine_ );
                bearing_ = Location::GetBearing( latitude, longitude,
                                                 intermediateLatitude_, intermediateLongitude_ );
                calcCompletion( intermediateLatitude_, intermediateLongitude_, latitudeSetting_,
                                longitudeSetting_, finalPerpendicularSlope_, finalStartOverLine_ );
            }
            else
            {
                bearing_ = Location::GetBearing( latitude, longitude,
                                                 latitudeSetting_, longitudeSetting_ );
                intermediateLatitude_ = latitudeSetting_;
                intermediateLongitude_ = longitudeSetting_;
            }
        }
    }

    // Set the waypoint / heading
    horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::WAYPOINT );
    bearingCmdWriter_->write( Units::RADIAN, bearing_ );
    latitudeCmdWriter_->write( Units::RADIAN, intermediateLatitude_ );
    longitudeCmdWriter_->write( Units::RADIAN, intermediateLongitude_ );
}

/// sets up node map and node queue
void HFRadarModelPoint::initNodes( const double& latitude, const double& longitude,
                                   int latSize, int lonSize, float* lat, float* lon,
                                   const float& maxCurrent )
{

    nodeQueue_.clear();
    nodeQueue_.put( startNode_->getFScore(), startNode_ );
    minNodeCount_ = 0;
    nodeMap_.clear();
    for( int iLat = 0; iLat < latSize; ++iLat )
    {
        nodeMap_[iLat] = new FlexArray<LocationNode*>( true, lonSize );
        for( int iLon = 0; iLon < lonSize; ++iLon )
        {
            LocationNode* newNode = new LocationNode( lat[iLat], lon[iLon], iLat,
                    iLon );
            nodeMap_[iLat]->set( iLon, newNode );
            //printf("At nodeMap_[%d]->get(%d), iLat=%d, iLon=%d\n",iLat,iLon,nodeMap_[iLat]->get(iLon)->getLatIndex(),nodeMap_[iLat]->get(iLon)->getLonIndex());
        }
    }
}

/// Returns the best possible travel time to goalNode_ following a straight line;
float HFRadarModelPoint::heuristic( LocationNode* node, const float& maxCurrent )
{
    //printf("Node at %08ZX, goalNode at %08ZX\n", (size_t)node, (size_t)goalNode_);
    return node->getDistance( *goalNode_ ) / ( maxCurrent + speedSetting_ );
}

// Returns 32 pairs of adjoining edge offsets from a grid center
// Can be subset at the first 8 or 16 offsets
const int HFRadarModelPoint::EDGE_OFFSETS[32][2] =
{
    {1, -1}, {1, 0}, {1, 1}, {0, -1}, { -1, -1}, {0, 1}, { -1, 1}, { -1, 0},
    {2, -1}, {2, 1}, {1, -2}, {1, 2}, { -1, -2}, { -1, 2}, { -2, -1}, { -2, 1},
    {3, -2}, {3, -1}, {3, 1}, {3, 2}, {2, -3}, {2, 3}, {1, -3}, {1, 3},
    { -1, -3}, { -1, 3}, { -2, -3}, { -2, 3}, { -3, -2}, { -3, -1}, { -3, 1}, { -3, 2}
};

/// Appends the adjoining nodes to the specified node to the FlexArray
void HFRadarModelPoint::getAdjoining( const LocationNode* node, FlexArray<LocationNode*>& adjs,
                                      int nLats, int nLons, float* lats, float* lons )
{
    int iLat0 = node->getLatIndex();
    int iLon0 = node->getLonIndex();

    // Special case of startNode_, find the nearest node;
    if( iLat0 == -1 )
    {
        float minDLat = 99999.9;
        for( int iLat = 0; iLat < nLats; ++iLat )
        {
            float dLat = fabs( lats[iLat] - node->getLat() );
            if( dLat < minDLat )
            {
                iLat0 = iLat;
                minDLat = dLat;
            }
        }
    }
    if( iLon0 == -1 )
    {
        float minDLon = 99999.9;
        for( int iLon = 0; iLon < nLons; ++iLon )
        {
            float dLon = fabs( lons[iLon] - node->getLon() );
            if( dLon < minDLon )
            {
                iLon0 = iLon;
                minDLon = dLon;
            }
        }
    }

    double maxDist = -1;
    for( int i = 0; i < 32; ++i )
    {
        int iLat = iLat0 + EDGE_OFFSETS[i][0];
        int iLon = iLon0 + EDGE_OFFSETS[i][1];
        if( iLat >= 0 && iLat < nLats && iLon >= 0 && iLon < nLons )
        {
            LocationNode* const adj = nodeMap_.get( iLat )->get( iLon );
            //printf("#### At nodeMap_.get(%d)->get(%d), iLat=%d, iLon=%d\n",iLat,iLon,adj->getLatIndex(),adj->getLonIndex());

            adjs.push( adj );
            // Look for intermediarte range grid locations
            if( i >= 8 && i < 16 )
            {
                double dist( adj->getDistanceFast( *node ) );
                if( dist > maxDist )
                {
                    maxDist = dist;
                }
            }
        }
    }

    if( node->getLatIndex() != -1 && node->getLonIndex() != -1
            && goalNode_->getDistanceFast( *node ) < maxDist )
    {
        adjs.push( goalNode_ );
    }
}

// Returns the travel time from one node to another
// Assumes that dScore of node equals arrival time at that node minus initTime_
float HFRadarModelPoint::travelTime( LocationNode* fromNode, LocationNode* toNode,
                                     const double* time, int timeSize, float*** uData, float*** vData )
{
    // Since start and goal nodes don't have velocities, assume equal to gridded values
    LocationNode* fromUVNode = fromNode == startNode_ ? toNode : fromNode;
    LocationNode* toUVNode = toNode == goalNode_ ? fromNode : toNode;

    // Get vector and distance
    double toEast, toNorth;
    fromNode->calcVector( *toNode, toEast, toNorth );
    double dist = sqrt( toEast * toEast + toNorth * toNorth );

    // Prepare to iterate for starting time
    float time0 = fromNode->getDScore();
    int tIndex0( 0 );
    // Don't extrapolate beyond the end of the forecast horizon.
    if( time0 > time[timeSize - 1] )
    {
        time0 = time[timeSize - 1];
        tIndex0 = timeSize - 2;
    }
    else for( ; time[tIndex0 + 1] - initTime_ <= time0 && tIndex0 + 2 < timeSize; ++tIndex0 );
    float tpath = 0;

    // Iterate several times.  The first time, just consider starting currents
    for( int i = 0; i < 3; ++i )
    {
        float u = AuvMath::Interpolate1D( time0,
                                          uData[tIndex0][fromUVNode->getLatIndex()][fromUVNode->getLonIndex()],
                                          uData[tIndex0 + 1][fromUVNode->getLatIndex()][fromUVNode->getLonIndex()],
                                          ( float )( time[tIndex0] - initTime_ ), ( float )( time[tIndex0 + 1] - initTime_ ) );
        float v = AuvMath::Interpolate1D( time0,
                                          vData[tIndex0][fromUVNode->getLatIndex()][fromUVNode->getLonIndex()],
                                          vData[tIndex0 + 1][fromUVNode->getLatIndex()][fromUVNode->getLonIndex()],
                                          ( float )( time[tIndex0] - initTime_ ), ( float )( time[tIndex0 + 1] - initTime_ ) );
        //printf("At i=%d, u=%g, v=%g\n",i,u,v);
        if( isnan( u ) || isnan( v ) )
        {
            return NAN;
        }
        if( i > 0 )
        {
            // First estimate, consider only starting point currents
            // Future estimates, average starting and ending currents
            float time1 = time0 + tpath;
            int tIndex1( 0 );
            // Don't extrapolate beyond the end of the forecast horizon.
            if( time1 > time[timeSize - 1] )
            {
                time1 = time[timeSize - 1];
                tIndex1 = timeSize - 2;
            }
            else for( ; time[tIndex1 + 1] - initTime_ <= time1 && tIndex1 + 2 < timeSize; ++tIndex1 );
            //printf("initTime_=%g, time[0]=%g, time[%d]=%g, time0=%g, tIndex0=%d, tpath=%g, time1=%g, tIndex1=%d\n",initTime_, time[0]-initTime_, timeSize-1, time[timeSize-1]-initTime_, time0, tIndex0, tpath, time1, tIndex1);
            //printf("toUVNode->getLatIndex()=%d,toUVNode->getLonIndex()=%d\n",toUVNode->getLatIndex(),toUVNode->getLonIndex());
            float uEnd = AuvMath::Interpolate1D( time1,
                                                 uData[tIndex1][toUVNode->getLatIndex()][toUVNode->getLonIndex()],
                                                 uData[tIndex1 + 1][toUVNode->getLatIndex()][toUVNode->getLonIndex()],
                                                 ( float )( time[tIndex1] - initTime_ ), ( float )( time[tIndex1 + 1] - initTime_ ) );
            float vEnd = AuvMath::Interpolate1D( time1,
                                                 vData[tIndex1][toUVNode->getLatIndex()][toUVNode->getLonIndex()],
                                                 vData[tIndex1 + 1][toUVNode->getLatIndex()][toUVNode->getLonIndex()],
                                                 ( float )( time[tIndex1] - initTime_ ), ( float )( time[tIndex1 + 1] - initTime_ ) );
            //printf("At i=%d, uEnd=%g, vEnd=%g\n",i,uEnd,vEnd);
            if( isnan( uEnd ) || isnan( vEnd ) )
            {
                return NAN;
            }
            // If we're going backwards at the end, it's a no-go
            if( i > 1 ) // Wait until the estimate is a little better than a first guess
            {
                double dotEnd = ( toEast * vEnd + toNorth * uEnd ) / dist;
                double disc = dotEnd * dotEnd + speedSetting_ * speedSetting_ - uEnd * vEnd - vEnd * uEnd;
                if( disc <= 0 )
                {
                    return NAN;
                }
            }
            u = 0.5 * ( u + uEnd );
            u = 0.5 * ( v + vEnd );
        }
        double dot = ( toEast * v + toNorth * u ) / dist;
        double disc = dot * dot + speedSetting_ * speedSetting_ - u * v - v * u;
        //printf( "toEast=%g, toNorth=%g, dot=%g, disc=%g\n", toEast, toNorth, dot, disc);
        if( disc <= 0 )
        {
            return NAN;
        }
        double vpath = dot + sqrt( disc );
        if( vpath < 0 )
        {
            return NAN;
        }
        tpath = dist / vpath;
    }
    //printf("For distance=%g, tpath=%g\n", dist, tpath);
    return tpath;
}

void HFRadarModelPoint::makeFine( int timeSize, int latSize, int lonSize,
                                  float* lat, float* lon, float*** u, float*** v )
{
    clearFine();
    int minILat = 99999;
    int maxILat = -1;
    int minILon = 99999;
    int maxILon = -1;
    LocationNode* node = goalNode_->getCameFrom();
    while( NULL != node->getCameFrom() )
    {
        int iLat = node->getLatIndex();
        if( iLat < minILat )
        {
            minILat = iLat;
        }
        if( iLat > maxILat )
        {
            maxILat = iLat;
        }
        int iLon = node->getLonIndex();
        if( iLon < minILon )
        {
            minILon = iLon;
        }
        if( iLon > maxILon )
        {
            maxILon = iLon;
        }
        node = node->getCameFrom();
    }
    minILat = AuvMath::Max( minILat - 4, 0 );
    maxILat = AuvMath::Min( maxILat + 4, latSize - 1 );
    minILon = AuvMath::Max( minILon - 4, 0 );
    maxILon = AuvMath::Min( maxILon + 4, lonSize - 1 );

    int gridDiv;
    if( !gridDivideSettingReader_->read( Units::COUNT, gridDiv ) )
    {
        gridDiv = 4;
    }
    fineLatSize_ = ( maxILat - minILat ) * gridDiv + 1;
    fineLat_ = new float[fineLatSize_];
    for( int iLat = 0; iLat < fineLatSize_; ++iLat )
    {
        int oLat = iLat / gridDiv + minILat;
        int rLat = iLat % gridDiv;
        if( 0 == rLat )
        {
            fineLat_[iLat] = lat[oLat];
        }
        else
        {
            fineLat_[iLat] = lat[oLat] * ( gridDiv - rLat ) / gridDiv
                             + lat[oLat + 1] * rLat / gridDiv;
        }
        //printf( "fineLat_[%d]=%g\n", iLat, fineLat_[iLat] );
    }

    fineLonSize_ = ( maxILon - minILon ) * gridDiv + 1;
    fineLon_ = new float[fineLonSize_];
    for( int iLon = 0; iLon < fineLonSize_; ++iLon )
    {
        int oLon = iLon / gridDiv + minILon;
        int rLon = iLon % gridDiv;
        if( 0 == rLon )
        {
            fineLon_[iLon] = lon[oLon];
        }
        else
        {
            fineLon_[iLon] = lon[oLon] * ( gridDiv - rLon ) / gridDiv
                             + lon[oLon + 1] * rLon / gridDiv;
        }
        //printf( "fineLon_[%d]=%g\n", iLon, fineLon_[iLon] );
    }

    fineU_ = new float**[timeSize];
    fineV_ = new float**[timeSize];
    for( int iTime = 0; iTime < timeSize; ++iTime )
    {
        fineU_[iTime] = new float*[fineLatSize_];
        fineV_[iTime] = new float*[fineLatSize_];
        for( int iLat = 0; iLat < fineLatSize_; ++iLat )
        {
            int oLat = iLat / gridDiv + minILat;
            int rLat = iLat % gridDiv;
            fineU_[iTime][iLat] = new float[fineLonSize_];
            fineV_[iTime][iLat] = new float[fineLonSize_];
            for( int iLon = 0; iLon < fineLonSize_; ++iLon )
            {
                int oLon = iLon / gridDiv + minILon;
                int rLon = iLon % gridDiv;
                if( rLat == 0 )
                {
                    if( rLon == 0 )
                    {
                        fineU_[iTime][iLat][iLon] = u[iTime][oLat][oLon];
                        fineV_[iTime][iLat][iLon] = v[iTime][oLat][oLon];
                    }
                    else
                    {
                        fineU_[iTime][iLat][iLon] = u[iTime][oLat][oLon]
                                                    * ( gridDiv - rLon ) / gridDiv
                                                    + u[iTime][oLat][oLon + 1] * rLon / gridDiv;
                        fineV_[iTime][iLat][iLon] = v[iTime][oLat][oLon]
                                                    * ( gridDiv - rLon ) / gridDiv
                                                    + v[iTime][oLat][oLon + 1] * rLon / gridDiv;
                    }
                }
                else
                {
                    if( rLon == 0 )
                    {
                        fineU_[iTime][iLat][iLon] = u[iTime][oLat][oLon]
                                                    * ( gridDiv - rLat ) / gridDiv
                                                    + u[iTime][oLat + 1][oLon] * rLat / gridDiv;
                        fineV_[iTime][iLat][iLon] = v[iTime][oLat][oLon]
                                                    * ( gridDiv - rLat ) / gridDiv
                                                    + v[iTime][oLat + 1][oLon] * rLat / gridDiv;
                    }
                    else
                    {
                        float uData[2][2] = { {
                                u[iTime][oLat][oLon],
                                u[iTime][oLat][oLon + 1]
                            }, {
                                u[iTime][oLat + 1][oLon],
                                u[iTime][oLat + 1][oLon + 1]
                            }
                        };
                        float vData[2][2] = { {
                                v[iTime][oLat][oLon],
                                v[iTime][oLat][oLon + 1]
                            }, {
                                v[iTime][oLat + 1][oLon],
                                v[iTime][oLat + 1][oLon + 1]
                            }
                        };
                        // If one and only one corner is NULL, mirror the
                        // opposite side so that half the sub-grid can be interpolated
                        int nanCount = ( isnan( uData[0][0] ) ? 1 : 0 )
                                       + ( isnan( uData[0][1] ) ? 1 : 0 )
                                       + ( isnan( uData[1][0] ) ? 1 : 0 )
                                       + ( isnan( uData[1][1] ) ? 1 : 0 );
                        if( nanCount == 1 )
                        {
                            if( isnan( uData[0][0] ) )
                            {
                                if( rLat + rLon >= gridDiv - 1 )
                                {
                                    uData[0][0] = uData[1][1];
                                    vData[0][0] = vData[1][1];
                                }
                            }
                            else if( isnan( uData[0][1] ) )
                            {
                                if( rLat + gridDiv - 1 - rLon >= gridDiv - 1 )
                                {
                                    uData[0][1] = uData[1][0];
                                    vData[0][1] = vData[1][0];
                                }
                            }
                            else if( isnan( uData[1][0] ) )
                            {
                                if( rLat + rLon <= gridDiv - 1 )
                                {
                                    uData[1][0] = uData[0][1];
                                    vData[1][0] = vData[0][1];
                                }
                            }
                            else if( isnan( uData[1][1] ) )
                            {
                                if( rLat + rLon <= gridDiv - 1 )
                                {
                                    uData[1][1] = uData[0][0];
                                    vData[1][1] = vData[0][0];
                                }
                            }
                        }
                        //if( iTime == 0 && oLat == 17 && oLon == 27 )
                        //{
                        //    printf( "uData=[%g,%g,%g,%g]\n",
                        //            uData[0][0], uData[0][1], uData[1][0], uData[1][1] );
                        //}
                        if( nanCount > 1 )
                        {
                            fineU_[iTime][iLat][iLon] = fineV_[iTime][iLat][iLon] = NAN;
                        }
                        else
                        {
                            fineU_[iTime][iLat][iLon] = AuvMath::Interpolate2D(
                                                            ( ( float ) rLat ) / gridDiv,
                                                            ( ( float ) rLon ) / gridDiv, uData, 0.0f, 1.0f,
                                                            0.0f, 1.0f );
                            fineV_[iTime][iLat][iLon] = AuvMath::Interpolate2D(
                                                            ( ( float ) rLat ) / gridDiv,
                                                            ( ( float ) rLon ) / gridDiv, vData, 0.0f, 1.0f,
                                                            0.0f, 1.0f );
                        }
                    }
                }
                //if( iTime == 0 && iLat >= fineLatSize_/2 - 8  && iLat <= fineLatSize_/2 +8 && iLon >= fineLonSize_/2 - 8  && iLon <= fineLonSize_/2 +8 )
                //{
                //    printf( "At oLat=%d,oLon=%d,iLat=%d,iLon=%d,rLat=%d,rLon=%d,u=%g,v=%g, oU's=[%g,%g,%g,%g]\n",
                //            oLat, oLon, iLat, iLon, rLat, rLon, fineU_[iTime][iLat][iLon], fineV_[iTime][iLat][iLon],
                //            u[iTime][oLat][oLon], u[iTime][oLat][oLon + 1], u[iTime][oLat + 1][oLon], u[iTime][oLat + 1][oLon + 1] );
                //}
            }
        }
    }
}

void HFRadarModelPoint::clearFine( void )
{
    if( NULL != fineLat_ )
    {
        delete[] fineLat_;
        fineLat_ = NULL;
    }
    if( NULL != fineLon_ )
    {
        delete[] fineLon_;
        fineLon_ = NULL;
    }
    for( int iTime = 0; iTime < timeSize_; ++iTime )
    {
        for( int iLat = 0; iLat < fineLatSize_; ++iLat )
        {
            if( NULL != fineU_ )
            {
                delete[] fineU_[iTime][iLat];
            }
            if( NULL != fineV_ )
            {
                delete[] fineV_[iTime][iLat];
            }
        }
        if( NULL != fineU_ )
        {
            delete[] fineU_[iTime];
        }
        if( NULL != fineV_ )
        {
            delete[] fineV_[iTime];
        }
    }
    if( NULL != fineU_ )
    {
        delete[] fineU_;
        fineU_ = NULL;
    }
    if( NULL != fineV_ )
    {
        delete[] fineV_;
        fineV_ = NULL;
    }
    fineLatSize_ = 0;
    fineLonSize_ =  0;
}

bool HFRadarModelPoint::runIfUnsatisfied( void )
{
    if( isSatisfied() )
    {
        logger_.syslog( "Reached Waypoint: " + Str( R2D( latitudeSetting_ ) ) + "," + Str( R2D( longitudeSetting_ ) ), Syslog::IMPORTANT );
        return true;
    }

    run();

    return false;
}

void HFRadarModelPoint::uninitialize()
{
    logger_.syslog( "Uninitialize HFRadarModelPoint." );

    latitudeReader_->requestData( false );
    longitudeReader_->requestData( false );

    initialized_ = false;
}

/// Mission Component factory interface
Behavior* HFRadarModelPoint::CreateBehavior( const Str& prefix, const Module* module )
{
    return new HFRadarModelPoint( prefix, module );
}