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

#ifndef RANGE_ESTIMATOR_H
#define RANGE_ESTIMATOR_H

#include <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;
    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);
    double RLS_Range(double MeasuredRange, double MeasuredUpperPressure, double MeasuredLowerPressure);

private:
    double ParseRangeMeasurements(double z);
};

#endif