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

#ifndef CURRENTESTIMATOR_H_
#define CURRENTESTIMATOR_H_

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

class UniversalDataReader;
class UniversalDataWriter;
class BlobReader;

/**
 *  Contains the CurrentEstimator component.  This component
 *  estimates water current speed and direction based on DVL
 *  bottom lock data.
 *
 *  CurrentEstimator.h should only be included by CurrentEstimator.cpp
 *  Other classes should include CurrentEstimatorIF.h
 *
 */
class CurrentEstimator : public Behavior
{
public:

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

    virtual ~CurrentEstimator();

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


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

    // Slate inputs
    UniversalBlobReader *vehicleOrientationReader_;
    UniversalBlobReader *velocityBottomReader_;
    UniversalBlobReader *velocityWaterReader_;
    DataReader* speedCalculatorReader_;

    // Slate outputs
    // Writers for the speed of the current in vehicle and earth frames
    DataWriter* currentSpeedVehicleWriter_;
    DataWriter* currentSpeedEarthWriter_;

    // Writers for the direction of the current in vehicle and earth frames
    DataWriter* currentDirectionVehicleWriter_;
    DataWriter* currentDirectionEarthWriter_;

    BlobWriter* vDirectionWriter_;
    BlobWriter* nDirectionWriter_;

    int verbosity_;
    Matrix3x3 rotationFromVehicleToNavigationFrame_;
    Point3D velocityRelativeToWaterInVehicleFrame_;
    Point3D velocityCurrentRelativeToGroundInVehicleFrame_;
    Point3D velocityCurrentRelativeToGroundInNavigationFrame_;
    Point3D velocityRelativeToGroundInVehicleFrame_;
    double speed_; // magnitude of vehicle velocity vector in meters per second
    double currentSpeedVF_, currentSpeedNF_, currentDirectionVF_, currentDirectionNF_; // members to hold what we write
    Timestamp dataStartTime_;

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

    void requestVehiclePose( bool req );

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

    bool estimateCurrent( void );

    void publishData();

};

#endif /*CurrentEstimator_H_*/