/** \file
 *
 *  Contains the ElevatorOffsetCalculator class declaration.
 *
 *  ElevatorOffsetCalculator.h should only be included by ElevatorOffsetCalculator.cpp
 *  Other classes should include ElevatorOffsetCalculatorIF.h
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef ELEVATOROFFSETCALCULATOR_H_
#define ELEVATOROFFSETCALCULATOR_H_

#include "ElevatorOffsetCalculatorIF.h"

#include <vector>
#include "component/SyncComponent.h"

class ConfigReader;
class UniversalDataReader;
class DataWriter;

class ElevatorOffsetCalculator : public SyncDerivationComponent
{
public:
    ElevatorOffsetCalculator( const Module* module );

    virtual ~ElevatorOffsetCalculator();

    /// Initialize function
    void initialize( void );

    /// Uninitialize function
    void uninitialize( void );

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

private:

    // Coeff value for exponential smoothing
    static const float EXP_SMOOTHING_COEFF;

    // Universal Slate inputs
    UniversalDataReader* depthReader_;
    UniversalDataReader* depthRateReader_;
    UniversalDataReader* pitchReader_;
    UniversalDataReader* elevatorAngleReader_;

    // Control Slate inputs
    DataReader* speedCmdReader_;
    DataReader* cmdPitchReader_;
    DataReader* massPositionCmdReader_;

    // Configuration
    ConfigReader *targetErrorBoundCfgReader_;
    ConfigReader *targetConfidenceLevelCfgReader_;
    ConfigReader *verbosityCfgReader_;
    ConfigReader *surfaceThresholdCfgReader_;

    // Slate writers
    DataWriter* elevatorAngleAverageWriter_;
    DataWriter* elevatorAngleVarianceWriter_;
    DataWriter* elevatorAngleErrorBoundWriter_;
    DataWriter* elevatorAngleCmdSpeedIDWriter_;
    DataWriter* elevatorAngleCmdPitchIDWriter_;
    DataWriter* elevatorAngleCmdMassPositionIDWriter_;

    // Configuration variables
    int verbosity_;
    float targetErrorBound_;  // Desired epsilon
    float targetConfidenceLevel_;
    float surfaceThreshold_;

    // Indicates initialization success
    bool initialized_;

    // Vehicle variables
    float depth_, pitch_, eleAngle_, cmdSpeed_, cmdPitch_, cmdMassPosition_;

    // Enables reporting to syslog and slate
    bool report_;

    // Elevator offset estimator variables
    struct elevatorAngleEstimator
    {
        elevatorAngleEstimator( const float cmdSpeedID, const float cmdPitchID, const float cmdMassPosID );

        ~elevatorAngleEstimator();

        // Estimator identifiers
        float cmdSpeedID_;
        float cmdPitchID_;
        float cmdMassPosID_;

        // Time keeping variables
        Timestamp startTime_;

        // Estimation variables
        int sampleSize_;
        double weightAccum_, mean_, varAccum_, variance_, errorBound_;
    };

    // Holds active estimators up to maxActiveEstimators_
    std::vector<elevatorAngleEstimator> activeEstimators_;
    size_t maxActiveEstimators_;
    float minEstimationTime_;  // minimum time to run an estimator before reporting
    float estimationTimeout_;  // maximum time to run an estimator

    double computeSampleWeight( void );

    void estimateWeightedAverageAndVariance( elevatorAngleEstimator &elevAngEst, float &sample, double &weight );

    void estimateWeightedErrorBound( elevatorAngleEstimator &elevAngEst );

    int activateEstimator( void );

    int addEstimator( void );

    bool isSatisfied( elevatorAngleEstimator &elevAngEst );

    bool readConfig( void );

    bool readData( void );

    void writeData( elevatorAngleEstimator &runAvg );

};

#endif /* ELEVATOROFFSETCALCULATOR_H_ */