//////////////////////////////////////////////////////////////////////////////
// Copyright 2018 MBARI.                                                    //
// Monterey Bay Aquarium Research Institute Proprietary Information.        //
// All rights reserved.                                                     //
//////////////////////////////////////////////////////////////////////////////

#ifndef RANGE_ESTIMATOR_H
#define RANGE_ESTIMATOR_H

#include <eigen3/Eigen/Dense>

using namespace std;
using namespace Eigen;

#define PISTON_DIAM 5.0
#define ROD_DIAM 1.5

class RangeEstimator {
    Matrix<double, 2, 2 > P;
    Matrix<double, 1, 1 > R;

private:
    double Force;
    double Au, Al;
    double gamma, a;
    Matrix<double, 2, 1 > xHat;
    Matrix<double, 2, 2 > eye;
    Matrix<double, 1, 2 > H;
    Matrix<double, 2, 1 > K;


public:
    RangeEstimator();
    RangeEstimator(double sigma, double cov1, double cov2);
    void SetParameters(double sigma, double cov1, double cov2);
    void GetxHat(double *xHat0, double *xHat1) { *xHat0 = xHat(0); *xHat1 = xHat(1); }
    double RLS_Range(double MeasuredRange, double MeasuredUpperPressure, double MeasuredLowerPressure);

private:
    double ParseRangeMeasurements(double z);
};

#endif
