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

#ifndef LBLNAVIGATION_H_
#define LBLNAVIGATION_H_

#include "component/SyncComponent.h"
#include "data/Location.h"
#include "data/Point2D.h"

#include "navigationModule/LBLNavigationIF.h"

class BlobDataReader;
class DataReader;
class UniversalDataReader;
class UniversalDataWriter;

/**
 *  LBLNavigation component
 *
 *  LBLNavigation.h should only be included by LBLNavigation.cpp
 *  Other classes should include LBLNavigationIF.h
 *
 *  \ingroup modules_navigation
 */
class LBLNavigation : public SyncComponent
{
public:
    LBLNavigation( const Module* module );
    virtual ~LBLNavigation();

    /// Initialize function
    void initialize( void );

    /// Ininitialize function
    void uninitialize( void );

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

protected:

    // Configuration

    /// Default speed of sound in seawater
    ConfigReader* verbosityConfigReader_;
    ConfigReader* soundSpeedConfigReader_;
    ConfigReader* navigationErrorConfigReader_;
    ConfigReader* baselineLockoutConfigReader_;
    ConfigReader* advancePingConfigReader_;
    ConfigReader* advancePositionConfigReader_;  // Fraction of change in location due to fix to apply (0 to 1)
    ConfigReader* maxPositionChangeConfigReader_;  // Maximum change in location due to fix
    ConfigReader* maxSpeedWrtTransponderConfigReader_;  // Maximum allowable speed between vehicle and transponder
    ConfigReader* pingFilterSizeConfigReader_;  // Size of median filter used with maxSpeedWrtTransponder
    ConfigReader* maxPingAgeInFilterConfigReader_;  // Pings older than this age will be removed from maxSpeedWrtTransponder filter
    ConfigReader* maxPingAgeConfigReader_;  // Pings older than this age will be ignored in position estimation
    ConfigReader* fixFilterSizeConfigReader_;  // Size of postional filter used calm noisy fixes (1 to 20)
    ConfigReader* fixHalfLifeInFilterConfigReader_;  // Fixes effect on the filter will be halved in this time
    // Slate input measurements

    /// Current platform latitude
    UniversalDataReader *latitudeReader_;

    /// Current platform longitude
    UniversalDataReader *longitudeReader_;

    /// Current platform depth
    UniversalDataReader *depthReader_;

    /// Current platform speed (from DR)
    UniversalDataReader *speedReader_;

    /// Current platform course (from DR)
    UniversalDataReader *courseReader_;

    /// Speed of sound in seawater
    UniversalDataReader* soundSpeedReader_;

    /// Ping location (origin)
    BlobReader *pingPositionReaders_[LBLNavigationIF::NUM_PINGS];

    /// Ping distance s
    DataReader *pingTOFReaders_[LBLNavigationIF::NUM_PINGS];

    // Slate output variables

    /// Current platform latitude
    UniversalDataWriter *latitudeWriter_;

    /// Current platform longitude
    UniversalDataWriter *longitudeWriter_;

    // Configuration inputs
    int verbosityCfg_;
    double navigationErrorCfg_;
    double baselineLockoutCfg_;
    double advancePingCfg_;
    double advancePositionCfg_;  // Fraction of change in location due to fix to apply (0 to 1)
    double maxPositionChangeCfg_;  // Maximum of change in location due to fix
    double maxSpeedWrtTransponderCfg_;  // Maximum allowable speed between vehicle and transponder
    int pingFilterSizeCfg_;  // Size of median filter used with maxSpeedWrtTransponder
    int lastPingFilterSizeCfg_;
    double maxPingAgeInFilterCfg_;  // Pings older than this age will be removed from maxSpeedWrtTransponder filter
    double maxPingAgeCfg_;  // Pings older than this age will be ignored in position estimation
    int fixFilterSizeCfg_;  // Size of postional filter used calm noisy fixes (1 to 20)
    int lastFixFilterSizeCfg_;
    int fixHalfLifeInFilterCfg_;  // Fixes effect on the filter will be halved in this time

    /// Slate inputs
    double soundSpeed_;

    // Last ping information (Updated after each ping)
    Point3D pingPositions_[LBLNavigationIF::NUM_PINGS];
    Location lastPingLocation_;
    double lastPingDistance_;
    double lastPingTimestamp_;
    int lastPingChannel_;
    void uninitializeLastPing();

    // maxSpeedWrtTransponder filter
    struct TimeAndDistance
    {
        double timestamp_;
        double distance_;
    };
    // These are static to simplfy unit testing of this essentially singleton component
    static TimeAndDistance PingFilter_[LBLNavigationIF::NUM_PINGS][LBLNavigationIF::MAX_PING_FILTER_SIZE + 1];
    static int PingFilterPos_[LBLNavigationIF::NUM_PINGS];

    // fixFilter
    struct TimeAndPoisition
    {
        double timestamp_;
        Point2D position_;
    };
    // These are static to simplfy unit testing of this essentially singleton component
    static TimeAndPoisition FixFilter_[LBLNavigationIF::MAX_FIX_FILTER_SIZE];
    static int FixFilterPos_;

public:

    static bool DoLBL( Location& location, double depth,
                       Location pingLocation, double pingDistance, Location lastPingLocation, double lastPingDistance,
                       double baselineLockoutCfg, LoggerBase& logger );

    static double CalcDistanceDistanceFromTOF( double tof, double pingDepth, double depth, double soundSpeed );

    // reset ping filters, either after initialization or after filter size change
    static void ResetPingFilters( int verbosity, LoggerBase& logger );

    // returns true if ping meets filter conditions
    static int  FilterPing( const int& iPing, const double& pingTime, const double& pingDistance,
                            int pingFilterSize, double maxPingAgeInFilter, double maxSpeedWrtTransponder,
                            int verbosity, LoggerBase& logger );

    // reset ping filters, either after initialization or after filter size change
    static void ResetFixFilters( int verbosity, LoggerBase& logger );

    // updates location by past filtered fixes
    static void FilterFix( const Location& currentLocation, Location& fix, const double& fixTime,
                           int fixFilterSize, double fixHalfLifeInFilter, double advancePosition, double maxPositionChange,
                           int verbosity, LoggerBase& logger );

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

};

#endif /* LBLNAVIGATION_H_ */