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

#ifndef BALLASTANDTRIM_H_
#define BALLASTANDTRIM_H_

#include "component/Behavior.h"
#include "data/Location.h"

class UniversalDataReader;

/**
 *  Contains the BallastAndTrim component.
 *
 *
 *  BallastAndTrim.h should only be included by BallastAndTrim.cpp
 *  Other classes should include BallastAndTrimIF.h
 *
 *  \ingroup modules_guidance
 */
class BallastAndTrim : public Behavior
{
public:

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

    virtual ~BallastAndTrim();

    /// Initialize function
    void initialize( void );

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

    /// Runs and returns false if B&T timer has not expired
    /// Otherwise returns true
    bool runIfUnsatisfied( void );

    /// Returns true when B&T estimates have coverged
    bool isSatisfied( void );

    /// Uninit function
    void uninitialize( void );

    /// Mission Component factory interface
    static Behavior* CreateBehavior( const Str& prefix, const Module* module );

protected:

    // XML inputs
    SettingReader *estimationTimeoutSettingReader_;
    SettingReader *minEstimationTimeSettingReader_;
    SettingReader *massConvergenceErrorSettingReader_;
    SettingReader *buoyConvergenceErrorSettingReader_;
    SettingReader *confidenceSettingReader_;

    // Return true if the waiting time is over
    bool calcSatisfied();

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

    // Slate inputs
    UniversalDataReader *massPositionReader_;
    UniversalDataReader *buoyancyPositionReader_;

    // Slate outputs
    DataWriter* massPositionMeanWriter_;
    DataWriter* massPositionVarianceWriter_;
    //DataWriter* massPositionConfidenceWriter_;
    DataWriter* buoyancyPositionMeanWriter_;
    DataWriter* buoyancyPositionVarianceWriter_;
    //DataWriter* buoyancyPositionConfidenceWriter_;

    // Time keeping variables
    Timestamp startTime_;
    Timespan timeoutDuration_;
    Timespan minDuration_;
    float timeoutDurationSeconds_;
    float minDurationSeconds_;

    Timestamp dataTimestamp_;

    // Indicates initialization success
    bool initialized_;

    // Actuator positions
    double massPos_, buoyPos_;

    // Estimator variables
    int sampleSize_;
    bool satisfied_;
    bool successful_; // Was the B&T actually successful
    float targetConfidenceLevel_;
    double massTargetErrorBound_, buoyTargetErrorBound_;
    double massPosMean_, buoyPosMean_;
    double massPosVarianceAccumilator_, buoyPosVarianceAccumilator_;
    double massPosVariance_, buoyPosVariance_;


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

    bool readData( void );

    void writeData( void );

    // Welford algorithm for online mean and varinace (see Knuth TAOCP vol 2, 3rd edition, page 232)
    void estimate( const double pos, double& mean,  double& varAccum, double& var );

    // Compute error bounds for the estimator (within target confidence level) using Chebyshev's inequality
    double epsilon( double& var );

};

#endif /* BALLASTANDTRIM_H_ */