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

#include <iostream>
#include <Eigen/Dense>
#include "RangeEstimator.hpp"

RangeEstimator::RangeEstimator() {
    eye(0, 0) = 1;
    eye(0, 1) = 0;
    eye(1, 0) = 0;
    eye(1, 1) = 1;
    R(0, 0) = pow(0.5, 2);
    Au = 0.25 * M_PI * PISTON_DIAM*PISTON_DIAM;
    Al = Au - 0.25 * M_PI * ROD_DIAM*ROD_DIAM;

    xHat(0) = -.025; //Generic Initial Condition for Force/Range Slope (1m = ~1500lbs)
    xHat(1) = 0; //Generic Initial Condition for position at zero force

    P(0, 0) = 1e-6;
    P(0, 1) = 0;
    P(1, 0) = 0;
    P(1, 1) = .2;
}

RangeEstimator::RangeEstimator(double sigma, double cov1, double cov2) {
    eye(0, 0) = 1;
    eye(0, 1) = 0;
    eye(1, 0) = 0;
    eye(1, 1) = 1;
    R(0, 0) = pow(sigma, 2);
    Au = 0.25 * M_PI * PISTON_DIAM*PISTON_DIAM;
    Al = Au - 0.25 * M_PI * ROD_DIAM*ROD_DIAM;

    xHat(0) = -.025; //Generic Initial Condition for Force/Range Slope (1m = ~1500lbs)
    xHat(1) = 0; //Generic Initial Condition for position at zero force

    P(0, 0) = cov1;
    P(0, 1) = 0;
    P(1, 0) = 0;
    P(1, 1) = cov2;
}



#define EPSILON 0.1
#define POINT_COUNT_TIMEOUT 20
#define N 3 //Number of differences to compute and compare (Must be > 1)

int sign(double x) {
    int ret_value = 0;
    if (x > 0)
        ret_value = 1;
    if (x < 0)
        ret_value = -1;
    return ret_value;
}

double RangeEstimator::ParseRangeMeasurements(double z) 
{
    static double z_last;
    static int delta[N] = {0};
    static int InvalidPointCtr = 0;

    double z_meas;
    int n;

    for (n = N - 1; n > 0; n--)
        delta[n] = delta[n - 1]; //Move previously computed deltas over
    delta[0] = sign(z - z_last); //Determine sign of change;
    if (fabs(z - z_last) < EPSILON) //If change is small enough, mark as no-change
        delta[0] = 0;
    z_last = z;


    //Valid point if the most recent change is small, and previous two are large and of the same sign
    if (((delta[0] == 0) && delta[1] * delta[2]) || (InvalidPointCtr++ == POINT_COUNT_TIMEOUT)) 
      {
        z_meas = z;
        InvalidPointCtr = 0;
      } 
    else
      z_meas = -999;

    return z_meas;
}

double RangeEstimator::RLS_Range(double MeasuredRange, double MeasuredUpperPressure, double MeasuredLowerPressure) {
    double Force;
    double Range;
    MeasuredRange = ParseRangeMeasurements(MeasuredRange); //Returns -99 if this data-point isn't a new valid measurement
    Force = Au * MeasuredUpperPressure - Al*MeasuredLowerPressure; //Compute Force on Piston based on upper and lower pressure measurements

    if (MeasuredRange > -999.0) //New Valid Range Measurement Available, use Recursive Least Squares to estimate slope and y-intercept of linear Range versus Force relationship
    {
        H(0) = Force;
        H(1) = 1.0;
        K = P * (H.transpose())*((H * P * H.transpose() + R).inverse());
        xHat = xHat + K * (MeasuredRange - H * xHat);
        P = (eye - K * H) * P * ((eye - K * H).transpose()) + K * R * (K.transpose());
    }


    Range = xHat(0) * Force + xHat(1); //Use most recent linear Range versus Force relationship to estimate current range based on pressure measurements
    return Range;
}


#ifdef TEST_ESTIMATOR
//g++ RangeEstimator.cpp -lm -DTEST_ESTIMATOR -o RangeEstimatorTest
//usage:  RangeEstimateTest < inputfile.txt > outputfile.txt

int main(int argc, char *argv[]) {
    float z;
    int Max = 40000;
    double z_meas, Pu_meas, Pl_meas;
    RangeEstimator Est(.5, 1e-6, .2);

    while (Max--) {
        cin >> z_meas >> Pu_meas >> Pl_meas;
        z = Est.RLS_Range(z_meas, Pu_meas, Pl_meas);
        cout << z << "  " << z_meas << "  " << Pu_meas << "  " << Pl_meas << endl;
    }
}

#endif