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

#include <iostream>
#include <stdio.h>
#include <unistd.h>
#include "RangeEstimator.hpp"

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

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

    this->SetParameters(0.995,1e-6,.2);
}

RangeEstimator::RangeEstimator(double gammaIn, double cov1, double cov2) {
    eye(0, 0) = 1;
    eye(0, 1) = 0;
    eye(1, 0) = 0;
    eye(1, 1) = 1;
    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

    this->SetParameters(gammaIn,cov1,cov2);
}

void RangeEstimator::SetParameters(double gammaIn, double cov1, double cov2) { 
    P(0, 0) = cov1;
    P(0, 1) = 0;
    P(1, 0) = 0;
    P(1, 1) = cov2;
    R(0, 0) = 1;
    gamma = gammaIn;
    a = 1 - gamma;
    //
    // The following sets it back to non-forgetting noisy-measurment RLS:
    //R(0, 0) = pow(sigma, 2);
    //gamma = 1;
    //a     = 1;
}

#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 = NAN;

    return z_meas;
}

double RangeEstimator::RLS_Range(double MeasuredRange, double MeasuredUpperPressure, double MeasuredLowerPressure) {
    double Force;
    double Range;
    MeasuredRange = ParseRangeMeasurements(MeasuredRange); //Returns NaN 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 (!isnan(MeasuredRange)) //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()/gamma)*((H * P * H.transpose()/gamma + R/a).inverse());
        xHat = xHat + K * (MeasuredRange - H * xHat);
        P = (eye - K * H) * P * ((eye - K * H).transpose())/gamma + K * R * (K.transpose())/a;
        printf("xHat:  %.4f  %.4f \n",xHat(0),xHat(1));
    }

    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;
    int foo,foo2;

    RangeEstimator Est; //(.995, 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;
        usleep(100000):
    }
}

#endif