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

#ifndef TRACKACOUSTICCONTACT_H_
#define TRACKACOUSTICCONTACT_H_

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

class UniversalDataReader;
class UniversalDataWriter;
class BlobReader;

/**
 *  Contains the TrackAcousticContact behavior.
 *  TrackAcousticContact transforms acoustic contact measurements from the
 *  Forward-Starboard-Keelward (FSK) vehicle-frame to the North-East-Down (NED)
 *  geographic frame. This behavior outputs the position of each contact to the
 *  slate, in both absolute and relative coordinates. Median filtered position
 *  estimates will also be outputted when numFixesLowPassSetting > 1 is passed into
 *  the behavior.
 *
 *  The behavior expects the acoustic contact data in the following format:
 *      - Direction to contact as a unit-less 3D unit vector in FSK
 *      - Slant range to contact in meters
 *
 *  The behavior outputs relative and absolute geographic position estimates for
 *  the acoustic contact that are intended for use by other components of the
 *  guidance module (e.g., terminal homing, circling).
 *
 *  /todo Implement tracking of multiple contacts (e.g., DAT address 1,2,3,...,n).
 *  /todo Add tracking _quality_ metric to output (e.g., mean and variance for a
 *        series of multiple measurements from a single contact).
 *
 *  TrackAcousticContact.h should only be included by TrackAcousticContact.cpp
 *  Other classes should include TrackAcousticContactIF.h
 *
 */
class TrackAcousticContact : public Behavior
{
public:

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

    virtual ~TrackAcousticContact();

    /// 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:

    /// Mission input settings
    SettingReader *contactLabelSettingReader_; // e.g., 0
    SettingReader *contactDepthSettingReader_; // e.g., 0 meter
    SettingReader *numFixesLowPassSettingReader_; // e.g., 25 count
    SettingReader *lowPassOverlapSettingReader_; // e.g., flase
    SettingReader *numSamplesReader_; // e.g., 20 count
    SettingReader *numStartingFixesToIgnoreSettingReader_; // 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.
    TrackAcousticContact( const TrackAcousticContact& old ); // disallow copy constructor

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

    /// Slate outputs
    DataWriter* contactLabelWriter_;
    DataWriter* contactLatitudeWriter_;
    DataWriter* contactLongitudeWriter_;
    DataWriter* contactDepthWriter_;
    DataWriter* rangeToContactWriter_;
    DataWriter* azimuthToContactWriter_;
    DataWriter* elevationToContactWriter_;
    DataWriter* headingToContactWriter_;
    DataWriter* eastingsToContactWriter_;
    DataWriter* northingsToContactWriter_;

    DataWriter* headingToContactLowPassWriter_;
    DataWriter* rangeToContactLowPassWriter_;
    DataWriter* contactLatitudeLowPassWriter_;
    DataWriter* contactLongitudeLowPassWriter_;
    DataWriter* contactDepthLowPassWriter_;

    BlobWriter* vDirectionWriter_;
    BlobWriter* nDirectionWriter_;
    BlobWriter* vRelPosWriter_;
    BlobWriter* nRelPosWriter_;

    /// Request parameters to pass to DAT/DUSBL driver
    DataWriter *requestContactLabelWriter_;
    DataWriter *requestNumPingsWriter_;

    /// Behavior settings
    int contactLabelSetting_, numberOfSamplesSetting_, numFixesLowPassSetting_, numStartingFixesToIgnoreSetting_;

    /// Incoming acoustic contact data
    int contactLabel_, contactUpdateCount_;
    double rangeToContact_, contactLatitude_, contactLongitude_, contactDepth_;
    Timestamp contactRxTime_;

    /// Smoothed acoustic contact data
    double contactLatitudeLowPass_, contactLongitudeLowPass_, contactDepthLowPass_;
    double contactBearingLowPass_, contactHorizontalRangeLowPass_;

    /// FSK to NED transfomation matrix
    Matrix3x3 rotationFSK2NED_;

    /// FSK contact position descriptors
    Point3D directionFSK_, relPosFSK_;
    double azimuthFSK_, elevationFSK_;

    /// NED contact position descriptors
    Point3D directionNED_, relPosNED_;
    double bearingNED_, inclinationNED_;

    /// When elevation angle measurement is ignored and the acoustic contact's depth is preknown and fixed (e.g., a Wave Glider).
    double contactDepthSetting_;

    /// Temporal bin filters
    BinFilter1D<double> contactLatBin_;
    BinFilter1D<double> contactLonBin_;
    BinFilter1D<double> contactDepthBin_;

    /// Allows bin filters window overlap when true
    bool windowOverlap_;

    /// Time-keeping for ping request
    Timestamp queryTime_;

    /// Ping request time interval
    Timespan updatePeriod_;

    /// Behavior syslog verbosity
    int verbosity_;

    /// Requests vehicle position data
    void requestVehiclePose( bool req );

    /// Powers the DAT/DUSBL
    void requestAcousticData( bool req );

    /// Issues an addresed range/azimuth/elevation query
    void queryAcousticContact( void );

    /// Reads the behavior settings from the mission
    void readSettings();

    /// Reads incoming addresed range/azimuth/elevation response
    bool readAcousticData();

    /// Reads the vehicle position variables
    bool readVehicleParams( double &lat, double &lon, double &depth );

    /// Transform observations from FSK into NED coordinate frame, and
    ///(optionally) apply low-pass filtering.
    void processAcousticData( double vlat, double vlon, double vdepth );

    /// Applies a median filter to the contact position data
    void smoothAcousticData( double vlat, double vlon );

    /// Writes the contact position data to the Slate
    void publishData();

    /// Prints debug output to the Syslog
    void printDebug();

};

#endif /*TRACKACOUSTICCONTACT_H_*/