/** \file
 *
 *  Contains the Navigator class declaration.
 *
 *  Navigator.h should only be included by Navigator.cpp and children.
 *
 *  Copyright (c) 2013 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
*/
#ifndef NAVIGATOR_H_
#define NAVIGATOR_H_

// include parent class
#include "component/SyncComponent.h" // should include component/Component.h in turn
// include classes that define any object that is a member of this class
#include "data/Matrix3x3.h"
#include "data/Point3D.h"
// forward declare classes for which this class includes a pointer
class ConfigReader;
class UniversalDataReader;
class UniversalDataWriter;

/**
 * General navigator class.
 *
 * Navigator.h should only be included by Navigator.cpp and children.
 *
 * \ingroup modules_navigation
 */
class Navigator : public SyncNavigationComponent
{
public:

    friend class DeadReckoner;

    Navigator( const Str& name, const Module* module ); // TODO: Can we remove the module arg somehow? all navigators are going to be in the navigationModule.

    virtual ~Navigator();

    virtual void initialize( void )
    {}

    virtual void run( void )
    {}

protected:

    ConfigReader* verbosityLevelConfigReader_;
    ConfigReader* allowableFailuresConfigReader_;
    ConfigReader* accuracyPremultiplierConfigReader_;
    ConfigReader* orientationStaleAfterConfigReader_;
    ConfigReader* velocityStaleAfterConfigReader_;

    UniversalDataReader* timeFixReader_;
    UniversalDataReader* latitudeFixReader_;
    UniversalDataReader* longitudeFixReader_;
    UniversalDataReader* latitudeReader_;
    UniversalDataReader* longitudeReader_;
    UniversalBlobReader* vehicleOrientationReader_;

    UniversalDataWriter* latitudeWriter_;
    UniversalDataWriter* longitudeWriter_;
    UniversalDataWriter* depthWriter_;
    UniversalDataWriter* platformSpeedWriter_;
    UniversalDataWriter* platformCourseWriter_;
    UniversalDataWriter* horizontalPathLengthSinceLastFixWriter_;

    UniversalDataWriter* fixDistanceMadeGoodWriter_;
    UniversalDataWriter* fixHorizontalPathLengthSinceLastFixWriter_;
    UniversalDataWriter* fixResidualDistanceWriter_;
    UniversalDataWriter* fixResidualBearingWriter_;
    UniversalDataWriter* fixResidualPercentDistanceTraveledWriter_;

    DataReader* setNavTimeFixReader_;
    DataReader* setNavLatitudeFixReader_;
    DataReader* setNavLongitudeFixReader_;

    DataWriter* elapsedSinceOrientationReadWriter_;
    DataWriter* elapsedSinceVelocityReadWriter_;
    DataWriter* latitudeAccuracyWriter_;

    int verbosity_;
    int allowableFailures_;
    double accuracyPremultiplier_; // configuration variable to allow the user to make one navigator preferred over others
    int inputMeasurementStartupAllowance_; // seconds to wait after startup before considering lack of data a fault
    Timestamp startTime_, navTimeStamp_;
    // bool orientationIsValid_, velocityIsValid_; // flags to keep track of whether supporting data is valid and was read successfully
    Timestamp orientationReadTime_, velocityReadTime_; // timestamps to track when valid orientation and velocity data was last read
    Timespan orientationStaleAfter_, velocityStaleAfter_; // timspans to define when data is considered stale
    double timeFix_, latitudeFix_, longitudeFix_; // geographic position of most recent fix (GPS, maybe acoustic in future...)
    double lastTimeFix_, lastLatitudeFix_, lastLongitudeFix_; // geographic position of last fix (GPS, maybe acoustic in future...)
    double horizontalPathLengthSinceLastFix_; // length in meters of the path in the horizontal plane since the last external fix (used to trigger DR error calculations)
    Point3D velocityRelativeToGroundInVehicleFrame_; // self-explanatory
    Matrix3x3 rotationFromVehicleToNavigationFrame_; // vehicle orientation as a rotation matrix
    Point3D velocityRelativeToGroundInNavigationFrame_; // self-explanatory
    Point3D displacementRelativeToGroundInNavigationFrame_; // self-explanatory
    double latitude_, longitude_, depth_; // geographic position of new estimate
    double course_, courseAccuracy_; // direction the vehicle is moving (in the locally-level frame)
    double speed_, speedAccuracy_; // magnitude of vehicle velocity vector in meters per second
    double lastLatitude_, lastLongitude_, lastDepth_; // estimated geographic position at end of last cycle
    double timeAccuracy_, latitudeAccuracy_, longitudeAccuracy_, depthAccuracy_, horizontalPathLengthSinceLastFixAccuracy_; // accuracy of new estimate

    // read config parameters
    void readConfigs( void );

    // check for a position fix from an external source
    void readExternalFix( void );

    // check for a position fix from SetNav behavior
    void readSetNavFix( void );

    // get the orientation of the vehicle as a rotation matrix
    void readVehicleOrientation( void );

    virtual void readVehicleVelocity( void )
    {}

    // write the estimated position of the vehicle
    void writeVehiclePosition( void );

    // evaluate and report the residual between the position estimate and an external fix
    void evaluateAndRecordFixResidual( void );
};

#endif /* NAVIGATOR_H_ */