/** \file
 *
 *  Contains the Tracking class declaration.
 *
 *  Tracking.h should only be included by Tracking.cpp
 *  Other classes should include TrackingIF.h
 *
 *  Copyright (c) 2013 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef TRACKING_H_
#define TRACKING_H_

#include "component/Behavior.h"
#include "data/Location.h"
#include "logger/Logger.h"
#include "data/Matrix3x3.h"
#include "data/Point3D.h"
#include "data/Point6D.h"

class UniversalDataReader;
class UniversalDataWriter;
class BlobReader;

/**
 *  Contains the Tracking component.  Initially, this component
 *  just transforms DAT measurements from the vehicle frame to the
 *  geographic frame. This component outputs the position of each contact
 *  to the slate, in both absolute and relative coordinates. Those outputs
 *  can then be used by other components of the guidance module. For example,
 *  the Homing behavior would drive the LRAUV toward the contact, and the
 *  Circle behavior would make the LRAUV circle the contact. Escape or Intercept
 *  behaviors might also be useful in the future, but would probably warrant
 *  more complete estimation of the contact state to enable reasonable predictions
 *  of future contact state.
 *
 *  /todo Add tracking _quality_ metric to output (e.g., mean and variance for a series of multiple measurements from a single contact).
 *  /todo Implement tracking of contact with specified label (e.g., DAT address 1).
 *  /todo Implement simultaneous tracking of multiple contacts.
 *  /todo Implement range-only tracking (e.g., for configurations with acoustic modem installed but without DAT.).
 *
 *  Tracking.h should only be included by Tracking.cpp
 *  Other classes should include TrackingIF.h
 *
 */
class Tracking : public Behavior
{
public:

    Tracking( const Str& prefix, const Module* module );

    virtual ~Tracking();

    /// Initialize function
    void initialize( void );

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

    /// Uninit function
    void uninitialize( void );

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

protected:

    // XML input
    SettingReader *contactDepthReader_; // e.g., 0 meter
    SettingReader *contactLabelReader_; // e.g., DAT-0
    SettingReader *numSamplesReader_; // e.g., 20 count
    SettingReader *numFixesLowPassReader_; // e.g., 25 count
    SettingReader *numStartingFixesToIgnoreReader_; // e.g., 2 count
    SettingReader *updatePeriodReader_; // e.g., 5 minutes

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

    // Slate inputs
    DataReader *latitudeReader_;
    DataReader *longitudeReader_;
    DataReader *depthReader_;
    UniversalBlobReader *vehicleOrientationReader_;
    // inputs from DAT
    UniversalDataReader *contactAddressReader_;
    UniversalDataReader *rangeToContactReader_;
    UniversalBlobReader *directionToContactReader_;
//    DataReader *relPosOfContactReader_;

    // Slate outputs
    DataWriter* contactLabelWriter_;
    DataWriter* contactLatitudeWriter_;
    DataWriter* contactLatitudeLowPassWriter_;
    DataWriter* contactLongitudeWriter_;
    DataWriter* contactLongitudeLowPassWriter_;
    DataWriter* contactDepthWriter_;
    DataWriter* contactDepthLowPassWriter_;
    DataWriter* rangeToContactWriter_;
    DataWriter* azimuthToContactWriter_;
    DataWriter* elevationToContactWriter_;
    DataWriter* headingToContactWriter_;
    DataWriter* eastingsToContactWriter_;
    DataWriter* northingsToContactWriter_;
    BlobWriter* vDirectionWriter_;
    BlobWriter* nDirectionWriter_;
    BlobWriter* vRelPosWriter_; // TODO displacement instead of relPos?
    BlobWriter* nRelPosWriter_; // TODO displacement instead of relPos?

    // Parameters to pass to DAT driver
    DataWriter *benthosAddressWriter_;
    DataWriter *onewayModeRequestedWriter_;
    DataWriter *numPingsWriter_;

    int verbosity_, cntrVar_;
    Matrix3x3 rotationFromVehicleToNavigationFrame_;
    int contactLabel_, contactLabelSetting_, numberOfSamplesSetting_, numFixesLowPassSetting_, numStartingFixesToIgnoreSetting_, queryModeSetting_;
    Timestamp contactRxTime_;
    double rangeToContact_, contactLatitude_, contactLatitudeLowPass_, contactLongitude_, contactLongitudeLowPass_, contactDepth_, contactDepthLowPass_;
    float azimuthInFSK_, elevationInFSK_, azimuthInNED_, elevationInNED_, contactDepthSetting_;
    Point3D directionInFSK_, relPosInFSK_, directionInNED_, relPosInNED_;

#define MAX_WINDOW_LENGTH_LATLONDEP 1000
    float contactLatitudeInWindow_[MAX_WINDOW_LENGTH_LATLONDEP];
    float contactLongitudeInWindow_[MAX_WINDOW_LENGTH_LATLONDEP];
    float contactDepthInWindow_[MAX_WINDOW_LENGTH_LATLONDEP];

    // Timeouts for for pinging
    Timestamp dataStartTime_;
    Timespan updatePeriod_;

    // returns true if data is requested from one of the readers
    bool isDataRequested();

    void requestVehiclePose( bool req );

    void requestAcousticData( bool req );

    // reads settings from the mission
    void readSettings();

    // Query DAT for observations of contact
    void queryDAT();

    bool readFromDAT();

    /** Transform observations into North-East-Down coordinate frame, and
     * (optionally) average several observations.
     */
    bool processFromDAT();

    void writeData();

    /**
     *  Convert a direction represented as a unit vector to angles of
     *  azimuth and elevation.
     *
     *  (replicated functionality and method from current DAT driver)
     *
     *  \todo This is a generally useful conversion and should be made available at a higher level (maybe in AuvMath.cpp?) to avoid having multiple versions of the same method.
     *
     */
    void unitVectorToAzimuthAndElevation( Point3D &uhat, float &azimuth, float &elevation );

};

#endif /*TRACKING_H_*/