/*
 * accelCalibrator.h
 *
 *  Created on: 2020.11.28
 *      Author: JackyPan
 */
#pragma once
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __ACCELCALIBRATOR_H
#define __ACCELCALIBRATOR_H

#include "common.h"

#define ACCEL_CAL_MAX_NUM_PARAMS 9
#define ACCEL_CAL_TOLERANCE 0.1
#define MAX_ITERATIONS  50
enum accel_cal_status_t {
    ACCEL_CAL_NOT_STARTED=0,
    ACCEL_CAL_WAITING_FOR_ORIENTATION=1,
    ACCEL_CAL_COLLECTING_SAMPLE=2,
    ACCEL_CAL_SUCCESS=3,
    ACCEL_CAL_FAILED=4
};

enum accel_cal_fit_type_t {
    ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID=0,
    ACCEL_CAL_ELLIPSOID=1
};

enum accel_cal_step_t{
	ACCELCAL_VEHICLE_POS_LEVEL=1,
	ACCELCAL_VEHICLE_POS_LEFT,
	ACCELCAL_VEHICLE_POS_RIGHT,
	ACCELCAL_VEHICLE_POS_NOSEDOWN,
	ACCELCAL_VEHICLE_POS_NOSEUP,
	ACCELCAL_VEHICLE_POS_BACK
};

class AccelCalibrator {
public:
    AccelCalibrator();

    //Select options, initialise variables and initiate accel calibration
    void start(enum accel_cal_fit_type_t fit_type = ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID, uint8_t num_samples = 6, float sample_time = 0.5f);
    void start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time, Vector3f offset, Vector3f diag, Vector3f offdiag);

    // set Accel calibrator status to make itself ready for future accel cals
    void clear();

    // returns true if accel calibrator is running
    bool running();

    // set Accel calibrator to start collecting samples in the next cycle
    void collect_sample();

    // check if client's calibrator is active
    void check_for_timeout();

    // get diag,offset or offdiag parameters as per the selected fitting surface or request
    void get_calibration(Vector3f& offset) const;
    void get_calibration(Vector3f& offset, Vector3f& diag) const;
    void get_calibration(Vector3f& offset, Vector3f& diag, Vector3f& offdiag) const;


    // collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done
    void new_sample(const Vector3f& delta_velocity, float dt);

    // interface for LSq estimator to read sample buffer sent after conversion from delta velocity
    // to averaged acc over time
    bool get_sample(uint8_t i, Vector3f& s) const;

    // returns truen and sample corrected with diag offdiag parameters as calculated by LSq estimation procedure
    // returns false if no correct parameter exists to be applied along with existing sample without corrections
    bool get_sample_corrected(uint8_t i, Vector3f& s) const;

    // set tolerance bar for parameter fitness value to cross so as to be deemed as correct values
    void set_tolerance(float tolerance) { _conf_tolerance = tolerance; }

    // returns current state of accel calibrators
    enum accel_cal_status_t get_status() const { return _status; }

    // returns number of samples collected
    uint8_t get_num_samples_collected() const { return _samples_collected; }

    // returns mean squared fitness of sample points to the selected surface
    float get_fitness() const { return _fitness; }

    struct param_t {
        Vector3f offset;
        Vector3f diag;
        Vector3f offdiag;
    };

private:
    struct AccelSample {
        Vector3f delta_velocity;
        float delta_time;
    };
    typedef    VectorN<float, ACCEL_CAL_MAX_NUM_PARAMS> VectorP;

    union param_u {
        struct param_t s;
        VectorN<float, ACCEL_CAL_MAX_NUM_PARAMS> a;

        param_u() : a{}
        {
            static_assert(sizeof(*this) == sizeof(struct param_t),
                          "Invalid union members: sizes do not match");
        }
    };

    //configuration
    uint8_t _conf_num_samples;
    float _conf_sample_time;
    enum accel_cal_fit_type_t _conf_fit_type;
    float _conf_tolerance;

    // state
    accel_cal_status_t _status;
    struct AccelSample* _sample_buffer;
    uint8_t _samples_collected;
    union param_u _param;
    float _fitness;
    uint32_t _last_samp_frag_collected_ms;
    float _min_sample_dist;

    // private methods
    // check sanity of including the sample and add it to buffer if test is passed
    bool accept_sample(const Vector3f& sample);

    // reset to calibrator state before the start of calibration
    void reset_state();

    // sets status of calibrator and takes appropriate actions
    void set_status(enum accel_cal_status_t);

    // determines if the result is acceptable
    bool accept_result() const;

    // returns number of parameters are required for selected Fit type
    uint8_t get_num_params() const;

    // Function related to Gauss Newton Least square regression process
    float calc_residual(const Vector3f& sample, const struct param_t& params) const;
    float calc_mean_squared_residuals() const;
    float calc_mean_squared_residuals(const struct param_t& params) const;
    void calc_jacob(const Vector3f& sample, const struct param_t& params, VectorP& ret) const;
    void run_fit(uint8_t max_iterations, float& fitness);
};


#endif /* __ACCELCALIBRATOR_H */

