/** \file
 *
 *  Contains the HFRadarModelPoint class declaration.
 *
 *  HFRadarModelPoint.h should only be included by HFRadarModelPoint.cpp
 *  Other classes should include HFRadarModelPointIF.h
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef HFRADARMODELPOINT_H_
#define HFRADARMODELPOINT_H_

#include "component/Behavior.h"
#include "data/Mtx.h"
#include "utils/FlexArray.h"

class Location;
class LocationNode;
class UniversalDataReader;
class UniversalDataWriter;

/**
 *  Over simplified HFRadarModelPoint component
 *
 *  HFRadarModelPoint.h should only be included by HFRadarModelPoint.cpp
 *  Other classes should include HFRadarModelPointIF.h
 *
 *  Implements onboard models work.
 *
 *  \ingroup modules_derivation
 */
class HFRadarModelPoint : public Behavior
{
public:
    HFRadarModelPoint( const Str& prefix, const Module* module );

    virtual ~HFRadarModelPoint();

    void initialize( void );

    /// Returns true when vehicle has "arrived" at the waypoint
    bool isSatisfied();

    /// The actual "payload" of the component
    void run();

    /// runs only if unsatisfied
    bool runIfUnsatisfied();

    void uninitialize( void );

    /// Mission Component factory interface
    static Behavior* CreateBehavior( const Str& prefix, const Module* module );

protected:

    /// Desired Latitude
    double latitudeSetting_;

    /// Desired Longitude
    double longitudeSetting_;

    /// Expected speed
    float speedSetting_;

    /// Intermediate Latitude along the way
    double intermediateLatitude_;

    /// Intermediate Laongitude along the way
    double intermediateLongitude_;

    // Intermediate lat/lon points
    FlexArray<Location*> subpoints_;

    // If true, need to calculate subpoints
    bool needSubpoints_;

    // If true, currently calculating subpoints
    bool calculatingSubpoints_;

    // If true, currently refining subpoints
    bool refiningSubpoints_;

    /// Storage for data when refining data points
    int fineLatSize_;
    float* fineLat_;
    int fineLonSize_;
    float* fineLon_;
    float*** fineU_;
    float*** fineV_;

    /// Desired Bearing
    float bearing_;

    /// Slope for crossing line (thru wapoint)
    double finalPerpendicularSlope_;

    /// Is our starting point latitude > than the latitude of the starting
    /// longitude on the crossing line?
    bool finalStartOverLine_;

    /// Slope for crossing line (thru intermediate wapoint)
    double intermediatePerpendicularSlope_;

    /// Is our starting point latitude > than the latitude of the starting
    /// longitude on the intermediate crossing line?
    bool intermediateStartOverLine_;

    /// Indicates initialization success
    bool initialized_;

    /// Time of initialization
    double initTime_;

    /// Size of time array
    int timeSize_;

    // Slate input setting variables

    /// distance from the nominal waypoint set by other settings.
    /// Must be combined with distanceDeltaBearing!
    SettingReader *distanceDeltaSettingReader_;

    /// bearing of distance from the nominal waypoint set by other settings.
    /// Must be combined with distanceDelta!
    SettingReader *distanceDeltaBearingSettingReader_;

    /// Desired change in longitude to achieve, expressed as a distance.
    /// If specified with Longitude, added to the specified Longitude
    /// Otherwise added to the Longitude at initialization time
    /// This variable is used as a test for sequential completion
    SettingReader *eastingsDeltaSettingReader_;

    // Desired waypoint latitude
    SettingReader *latitudeSettingReader_;

    // Desired waypoint latitude delta from the current latitude
    // If specified with latitude, the two are added together
    SettingReader *latitudeDeltaSettingReader_;

    // Desired waypoint longitude
    SettingReader *longitudeSettingReader_;

    // Desired waypoint latitude delta from the current latitude
    // If specified with latitude, the two are added together
    SettingReader *longitudeDeltaSettingReader_;

    /// Desired change in latitude to achieve, expressed as a distance.
    /// If specified with Latitude, added to the specified Latitude
    /// Otherwise added to the Latitude at initialization time
    /// This variable is used as a test for sequential completion
    SettingReader *northingsDeltaSettingReader_;

    /// Speed of the vehicle to use in computations
    SettingReader *speedSettingReader_;

    /// Speed of the vehicle to use in computations
    SettingReader *gridDivideSettingReader_;

    // Slate input measurements

    /// Current platform latitude
    UniversalDataReader *latitudeReader_;

    /// Current platform longitude
    UniversalDataReader *longitudeReader_;

    // Slate output variables

    // Sets the heading mode for HeadingControl
    DataWriter *horizontalModeWriter_;

    // Sets the waypoint latitude for HeadingControl
    DataWriter *latitudeCmdWriter_;

    // Sets the waypoint longitude for HeadingControl
    DataWriter *longitudeCmdWriter_;

    // Sets the bearing to the waypoint
    DataWriter *bearingCmdWriter_;

    // Variables related to path search algorithm

    Timestamp calcCompleteTime_;

    LocationNode* startNode_;

    LocationNode* goalNode_;

    /// The node map
    FlexArray<FlexArray<LocationNode*>*> nodeMap_;

    /// The A* priority queue
    FastMap<const float, LocationNode*> nodeQueue_;

    /// Just for statistics, number of nodes considered "minimum" nodes
    int minNodeCount_;

    /// Returns the best possible travel time to goalNode_ following a straight line;
    float heuristic( LocationNode* node, const float& maxCurent );

    // Returns 32 pairs of adjoining edge offsets from a grid center
    // Can be subset at the first 8 or 16 offsets
    static const int EDGE_OFFSETS[32][2];

    /// Appends the adjoining nodes to the specified node to the FlexArray
    void getAdjoining( const LocationNode* node, FlexArray<LocationNode*>& adjs,
                       int nLats, int nLons, float* lats, float* lons );

    // Returns the travel time from one node to another
    // Assumes that dScore of node equals arrival time at that node minus initTime_
    float travelTime( LocationNode* fromNode, LocationNode* toNode,
                      const double* time, int timeSize, float*** u, float*** v );

private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    HFRadarModelPoint( const HFRadarModelPoint& old ); // disallow copy constructor

    void initCompletion( double startLatitude, double startLongitude );

    void calcCompletion( double startLatitude, double startLongitude,
                         double latitudeSetting, double longitudeSetting,
                         double& perpendicularSlope, bool& startOverLine );

    bool isComplete( double latitude, double longitude, double latitudeGoal,
                     double longitudeGoal, double perpendicularSlope,
                     bool startOverLine );

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

    // Allocates fine-grained data
    void makeFine( int timeSize, int latSize, int lonSize, float* lat,
                   float* lon, float*** u, float*** v );

    /// Clears refining data
    void clearFine();

};

#endif /*RATECALCULATOR_H_*/