////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2020-2021 Qualcomm Technologies, Inc.
// All Rights Reserved.
// Confidential and Proprietary - Qualcomm Technologies, Inc.
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#pragma once

#if defined(__DIS_LOAD_PARAMS__)
#include <stdio.h>
#include <iostream>
#endif

#include "datatypes.h"
#include "is_dbg_util.h"

#if defined(__EIS_FL_ALGO__)
#include "ransac.h"
#include "matrixbaseddis.h"
#include "homographywlsqsolver.h"
#include "biasestimator.h"
#include "eis_internal_cvp_header.h"
#endif /* __EIS_FL_ALGO__ */

#if defined(ANDROID) && defined(__EIS_FL_ALGO__) && defined (__EIS_NFL_ALGO__)
#error "both __EIS_FL_ALGO__ and __EIS_NFL_ALGO__ defined"
#endif

#if defined(_EIS_INTERNAL_DEBUG_)
class  dis_debug_data
{
public:
    #define FILE_NAME_orientation                   IS_DBG_UTILS_DEFAULT_DIR"dump_dis__orientation.csv"
    #define FILE_NAME_rotation                      IS_DBG_UTILS_DEFAULT_DIR"dump_dis__rotation.csv"
    #define FILE_NAME_camera_matrix                 IS_DBG_UTILS_DEFAULT_DIR"dump_dis__camera_matrix.csv"
    #define FILE_NAME_dis_raw                       IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dis_raw.csv"
    #define FILE_NAME_dis_filterd                   IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dis_filterd.csv"
    #define FILE_NAME_dis_final                     IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dis_final.csv"
    #define FILE_NAME_dis_angle                     IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dis_angle.csv"
    #define FILE_NAME_src_before_ransac             IS_DBG_UTILS_DEFAULT_DIR"dump_dis__src_before_ransac.csv"
    #define FILE_NAME_dst_before_ransac             IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dst_before_ransac.csv"
    #define FILE_NAME_src_after_ransac              IS_DBG_UTILS_DEFAULT_DIR"dump_dis__src_after_ransac.csv"
    #define FILE_NAME_dst_after_ransac              IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dst_after_ransac.csv"
    #define FILE_NAME_src_after_ransac_not_filterd  IS_DBG_UTILS_DEFAULT_DIR"dump_dis__src_after_ransac_not_filterd.csv"
    #define FILE_NAME_dst_after_ransac_not_filterd  IS_DBG_UTILS_DEFAULT_DIR"dump_dis__dst_after_ransac_not_filterd.csv"
    #define FILE_NAME_ransac_model                  IS_DBG_UTILS_DEFAULT_DIR"dump_dis__ransac_model.csv"
    #define FILE_NAME_frame_inputs                  IS_DBG_UTILS_DEFAULT_DIR"dump_dis__frame_inputs.csv"

private:
    FILE* _orientation;
    FILE* _rotation;
    FILE* _camera_matrix;
    FILE* _dis_raw;
    FILE* _dis_filterd;
    FILE* _dis_final;
    FILE* _dis_angle;
    FILE* _src_before_ransac;
    FILE* _dst_before_ransac;
    FILE* _src_after_ransac;
    FILE* _dst_after_ransac;
    FILE* _src_after_ransac_not_filterd;
    FILE* _dst_after_ransac_not_filterd;
    FILE* _ransac_model;
    FILE* _frame_inputs;

    bool _is_first_frame_inputs;

public:
    dis_debug_data(int32_t& status)
    :
        _orientation(NULL),
        _rotation(NULL),
        _camera_matrix(NULL),
        _dis_raw(NULL),
        _dis_filterd(NULL),
        _dis_angle(NULL),
        _src_before_ransac(NULL),
        _dst_before_ransac(NULL),
        _src_after_ransac(NULL),
        _dst_after_ransac(NULL),
        _src_after_ransac_not_filterd(NULL),
        _dst_after_ransac_not_filterd(NULL),
        _ransac_model(NULL),
        _frame_inputs(NULL),
        _is_first_frame_inputs(true)
    {
        IS_ASSERT(IS_RET_SUCCESS == status);
        errno_t err;

#if !defined(_WIN32) && !defined(_WIN64)
        IS_ERR("Compiling with internal EIS debug struct enabled");
#endif /* !_WIN32 && !_WIN64 */

        char output_csv[1024];

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_orientation);

            err = IS_FOPEN(&_orientation, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_rotation);

            err = IS_FOPEN(&_rotation, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_camera_matrix);

            err = IS_FOPEN(&_camera_matrix, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dis_raw);

            err = IS_FOPEN(&_dis_raw, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dis_filterd);

            err = IS_FOPEN(&_dis_filterd, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dis_final);

            err = IS_FOPEN(&_dis_final, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dis_angle);

            err = IS_FOPEN(&_dis_angle, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_src_before_ransac);

            err = IS_FOPEN(&_src_before_ransac, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dst_before_ransac);

            err = IS_FOPEN(&_dst_before_ransac, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_src_after_ransac);

            err = IS_FOPEN(&_src_after_ransac, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dst_after_ransac);

            err = IS_FOPEN(&_dst_after_ransac, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_src_after_ransac_not_filterd);

            err = IS_FOPEN(&_src_after_ransac_not_filterd, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_dst_after_ransac_not_filterd);

            err = IS_FOPEN(&_dst_after_ransac_not_filterd, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_ransac_model);

            err = IS_FOPEN(&_ransac_model, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }

        {
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s", g_output_csv_base_folder, FILE_NAME_frame_inputs);

            err = IS_FOPEN(&_frame_inputs, output_csv, "w");
            if (err != 0)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Cannot open file %s", output_csv);
                status = IS_RET_GENERAL_ERROR;
            }
        }
    }

    ~dis_debug_data()
    {
        IS_FCLOSE(_orientation);
        IS_FCLOSE(_rotation);
        IS_FCLOSE(_camera_matrix);
        IS_FCLOSE(_dis_raw);
        IS_FCLOSE(_dis_filterd);
        IS_FCLOSE(_dis_final);
        IS_FCLOSE(_dis_angle);
        IS_FCLOSE(_src_before_ransac);
        IS_FCLOSE(_dst_before_ransac);
        IS_FCLOSE(_src_after_ransac);
        IS_FCLOSE(_dst_after_ransac);
        IS_FCLOSE(_src_after_ransac_not_filterd);
        IS_FCLOSE(_dst_after_ransac_not_filterd);
        IS_FCLOSE(_ransac_model);
        IS_FCLOSE(_frame_inputs);
    }

    inline void orientation_add_line(uint32_t frame_id, const Hom& m)
    {
        print_mat(_orientation, frame_id, m);
    }

    void rotation_add_line(uint32_t frame_id, const Hom& m)
    {
        print_mat(_rotation, frame_id, m);
    }

    void camera_matrix_add_line(uint32_t frame_id, const Hom& m)
    {
        print_mat(_camera_matrix, frame_id, m);
    }

    void dis_raw_add_line(uint32_t frame_id, const Hom& m)
    {
        print_mat(_dis_raw, frame_id, m);
    }

    void dis_filterd_add_line(uint32_t frame_id, const Hom& m)
    {
        print_mat(_dis_filterd, frame_id, m);
    }

    void dis_final_add_line(uint32_t frame_id, const Hom& m)
    {
        print_mat(_dis_final, frame_id, m);
    }

    void dis_angle_add_line(const Ang& ang)
    {
        fprintf(_dis_angle, "%.15lf, %.15lf, %.15lf\n",
            ang.at(0), ang.at(1), ang.at(2));
    }

    void dis_angle_add_line(const Quat& quat)
    {
        fprintf(_dis_angle, "%.15lf, %.15lf, %.15lf, %.15lf\n",
            quat.w, quat.v.x, quat.v.y, quat.v.z);
    }

    void ransac_model_add_line(const EstimatorEssential2pt& model, double ratio)
    {
        fprintf(_ransac_model, "#\n");
        fprintf(_ransac_model, "%.15lf, %.15lf, %.15lf, %.15lf\n", model._coefs[0], model._coefs[1], model._coefs[2], ratio);
    }

    void dis_src_dst_add_line(const Match2d* _curr_match, uint32_t num_matches, uint32_t frame_id, uint32_t before_ransac)
    {
        FILE* f_src = NULL;
        FILE* f_dst = NULL;

        switch (before_ransac)
        {
        case 0:
            f_src = _src_after_ransac;
            f_dst = _dst_after_ransac;
            break;
        case 1:
            f_src = _src_before_ransac;
            f_dst = _dst_before_ransac;
            break;
        case 2:
            f_src = _src_after_ransac_not_filterd;
            f_dst = _dst_after_ransac_not_filterd;
            break;
        default:
            IS_ASSERT(0);
            break;
        }

        // src
        {
            for (size_t idx = 0; idx < num_matches; idx++)
            {
                const Point2d& point = _curr_match[idx]._src;
                fprintf(f_src, "%u, %.15lf, %.15lf, %u\n", frame_id, point.x, point.y, (uint32_t)_curr_match[idx]._status);
            }
        }

        // dst
        {
            for (size_t idx = 0; idx < num_matches; idx++)
            {
                const Point2d& point = _curr_match[idx]._dst;
                fprintf(f_dst, "%u, %.15lf, %.15lf, %u\n", frame_id, point.x, point.y, (uint32_t)_curr_match[idx]._status);
            }
        }
    }

    void add_frame_inputs(const FilterInput& frame_input_back)
    {
        if (true == _is_first_frame_inputs)
        {
            fprintf(_frame_inputs, "; sensor_idx, effective_readout_duration, exposure_time, "
                                    "frame_time, ts_start, ts_end, "
                                    "frame_id, dropped_frames_from_previous_frame, focus_distance_cm, "
                                    "scene_type, avg_ois_ang(0), avg_ois_ang(1), avg_ois_ang(2)\n");

                _is_first_frame_inputs  = false;
        }

        fprintf(_frame_inputs, "%u, %u, %u, "
                               "%u, %" PRIu64 ", %" PRIu64 ", "
                               "%u, %u, %u, "
                               "%u, %.15lf, %.15lf, %.15lf\n",
            frame_input_back.sensor_idx,
            frame_input_back.effective_readout_duration,
            frame_input_back.exposure_time,

            frame_input_back.frame_time,
            frame_input_back.ts_start,
            frame_input_back.ts_end,

            frame_input_back.frame_id,
            frame_input_back.dropped_frames_from_previous_frame,
            frame_input_back.focus_distance_cm,

            (uint32_t)frame_input_back.scene_type,
            frame_input_back.avg_ois_ang(0),
            frame_input_back.avg_ois_ang(1),
            frame_input_back.avg_ois_ang(2)
            );
    }

private:
    inline void print_mat(FILE* f, uint32_t frame_id, const Hom& m)
    {
        IS_ASSERT(NULL != f);
        (void)frame_id;

        fprintf(f, "%.15lf, %.15lf, %.15lf, %.15lf, %.15lf, %.15lf, %.15lf, %.15lf, %.15lf\n",
            m[0], m[1], m[2],
            m[3], m[4], m[5],
            m[6], m[7], m[8]);
    }

};

#define _camera_matrix_add_line(__debug, _frame_id, _mat)                           __debug->camera_matrix_add_line(_frame_id, _mat)
#define _dis_angle_add_line(_cond, __debug, _orientation)                           if (true == _cond) { __debug->dis_angle_add_line(_orientation); }
#define _orientation_add_line(__debug, _frame_id, _orient)                          __debug->orientation_add_line(_frame_id, _orient)
#define _rotation_add_line(__debug, _frame_id, _rotation)                           __debug->rotation_add_line(_frame_id ,_rotation)
#define _ransac_model_add_line(__debug, _model, _ratio)                             __debug->ransac_model_add_line(_model, _ratio)
#define _dis_raw_add_line(__debug, _frame_id, _mat)                                 __debug->dis_raw_add_line(_frame_id, _mat)
#define _dis_filterd_add_line(__debug, _frame_id, _mat)                             __debug->dis_filterd_add_line(_frame_id, _mat)
#define _dis_final_add_line(__debug, _frame_id, _mat)                               __debug->dis_final_add_line(_frame_id, _mat)
#define _dis_src_dst_add_line(__debug, _curr_match, _num_matches, _frame_id, _before_ransac)\
                                              __debug->dis_src_dst_add_line(_curr_match, _num_matches, _frame_id, _before_ransac)

#define _frame_inputs_add_line(__debug, _frame_input)                               __debug->add_frame_inputs(_frame_input)
#else
#define _camera_matrix_add_line(...)                        ((void)0)
#define _dis_angle_add_line(...)                            ((void)0)
#define _orientation_add_line(...)                          ((void)0)
#define _rotation_add_line(...)                             ((void)0)
#define _dis_src_dst_add_line(...)                          ((void)0)
#define _ransac_model_add_line(...)                         ((void)0)
#define _dis_raw_add_line(...)                              ((void)0)
#define _dis_filterd_add_line(...)                          ((void)0)
#define _dis_final_add_line(...)                            ((void)0)
#define _frame_inputs_add_line(...)                         ((void)0)
#endif /* _EIS_INTERNAL_DEBUG_ */

#if defined(__EIS_FL_ALGO__)
class DisStabilizer
{
private:
    // HW parameters
    static const uint32_t           _max_curr_match_size                    = HFI_DME_MAX_FPX;

    // Default algorithm values
    static const uint32_t           _min_matches                            = 75; //TODO (etsairi): need to be calibration value?
    static const uint32_t           _default_exposure_time_threshold_usec   = (uint32_t)(MILLI_TO_USEC(9.9));
    static const uint32_t           _default_focus_distance_threshold_cm    = 60U;

    class Params
    {
    public:
        uint32_t    _exposure_time_threshold_usec[EIS_MAX_NUM_SENSORS]; // Above this value DIS will be disabled
        uint32_t    _focus_distance_threshold_cm[EIS_MAX_NUM_SENSORS];  // Below this threshold DIS will be disabled
        uint32_t    _confidence_threshold[EIS_MAX_NUM_SENSORS];         // Above this threshold DIS will be disabled
        bool        _is_sensor_enabled[EIS_MAX_NUM_SENSORS];            // true if DIS is enabled for this sensor, otherwise false


    public:
        Params(
            const is_init_data_sensor*     init_sensors,
            uint32_t                       num_sensors)
        {
            for (uint32_t idx = 0; idx < ARR_SIZE(_exposure_time_threshold_usec); idx++)
            {
                _exposure_time_threshold_usec[idx] = _default_exposure_time_threshold_usec;
            }

            for (uint32_t idx = 0; idx < ARR_SIZE(_focus_distance_threshold_cm); idx++)
            {
                _focus_distance_threshold_cm[idx] = _default_focus_distance_threshold_cm;
            }

            for (uint32_t idx = 0; idx < ARR_SIZE(_is_sensor_enabled); idx++)
            {
                _is_sensor_enabled[idx] = false;  // Disable all sensors by default, enable only if chromatix has this values enabled
            }

            for (uint32_t idx = 0; idx < ARR_SIZE(_confidence_threshold); idx++)
            {
                _confidence_threshold[idx] = 255;
            }

            for (uint32_t idx = 0; idx < num_sensors; idx++)
            {
                if (0 != init_sensors[idx].is_chromatix_info.dis.enable)
                {
                    IS_ASSERT(1 == init_sensors[idx].is_chromatix_info.dis.enable);

                    _is_sensor_enabled[idx] = true;

                    _exposure_time_threshold_usec[idx] = (0 != init_sensors[idx].is_chromatix_info.dis__parameter_mode) ?
                        (uint32_t)SEC_TO_USEC(init_sensors[idx].is_chromatix_info.dis__exposure_time_threshold) : _default_exposure_time_threshold_usec;


                    _focus_distance_threshold_cm[idx] = (0 != init_sensors[idx].is_chromatix_info.dis__parameter_mode) ?
                        (uint32_t)METER_TO_CM(init_sensors[idx].is_chromatix_info.dis__focus_distance_threshold) : _default_focus_distance_threshold_cm;

                    _confidence_threshold[idx] = init_sensors[idx].is_chromatix_info.dis.confidence_threshold;
                }
            }
        }
    };

private:
    Params                                      _params;
    Msac<Match2d, EstimatorEssential2pt>        _ransac;
    HomographyWlsqSolver                        _solver;
    MatrixBasedDis                              _dis;
    BiasEstimator                               _bias_estimator;

    Hom                                         _prev_orientation;      /**< Last orientation calculated */

    // Motion vectors buffer
    Match2d                                     _curr_match[_max_curr_match_size];
    uint32_t                                    _curr_match_size;

#if defined(_EIS_INTERNAL_DEBUG_)
    dis_debug_data                              *_debug;
#endif /* _EIS_INTERNAL_DEBUG_ */

private:

    // scale and rotate MV's according to previous frame domain
    inline void construct_curr_match(
        const is_input_dis_t* const     dis_motion_vectors,
        const Hom&                      rotation_from_curr_to_prev,
        const Camera&                   camera,
        const ZoomWindows&              zoom)
    {
        // Upscale the MVs to match the image size
        const double scale_factor_width = (double)camera.im_w / dis_motion_vectors->motion_vectors_input_image_width;
        const double scale_factor_height = (double)camera.im_h / dis_motion_vectors->motion_vectors_input_image_height;

        const HFI_CVP_VIDEO_STATS_V2* mv = (HFI_CVP_VIDEO_STATS_V2*)dis_motion_vectors->motion_vectors;
        for (uint32_t idx = 0; idx < MIN(_max_curr_match_size, mv->nNumMv); idx++)
        {
            const Point2d src_tmp(mv->sdisInfo[idx].tarCenterX, mv->sdisInfo[idx].tarCenterY);  // Current frame
            const Point2d dst_tmp(mv->sdisInfo[idx].refCenterX, mv->sdisInfo[idx].refCenterY);  // Previous frame

            // Swap the MV's so that:
            //      src := previous frame
            //      dst := current frame
            const Point2d src((dst_tmp.x + 0.5) * scale_factor_width - 0.5, (dst_tmp.y + 0.5) * scale_factor_height - 0.5);
            const Point2d dst((src_tmp.x + 0.5) * scale_factor_width - 0.5, (src_tmp.y + 0.5) * scale_factor_height - 0.5);

            // Rotate the MVs, points are also scaled by focal length, happens inside unproject()
            {
                _curr_match[idx]._src = camera.unproject(src, zoom);
                _curr_match[idx]._dst = camera.unproject(rotation_from_curr_to_prev * dst, zoom); // Rotate current MV to previous frame and then un-project it
                _curr_match[idx]._status = (0 != mv->sdisInfo[idx].meta.data.isInlier) ? MatchStatus::CVP_INLIER : MatchStatus::CVP_OUTLIER;
            }
        }

        _curr_match_size = MIN(_max_curr_match_size, mv->nNumMv);
    }

    // return orientation from current frame to previous frame
    inline Hom get_current_rotation(const StabilizerDistortionCorrection* dist_corr) const
    {
        if (dist_corr->_frame_inputs.size() > 1)
        {
            const Camera&   camera      = dist_corr->_camera[dist_corr->_frame_inputs.back().sensor_idx];
            const Ang&      ang         = dist_corr->_frame_angles.at_history(0);
            const Ang&      ang_prev    = dist_corr->_frame_angles.at_history(1);

            IS_ASSERT(dist_corr->_frame_angles.at_history(0).at(0) == dist_corr->_frame_angles.back().at(0));
            IS_ASSERT(dist_corr->_frame_angles.at_history(0).at(1) == dist_corr->_frame_angles.back().at(1));
            IS_ASSERT(dist_corr->_frame_angles.at_history(0).at(2) == dist_corr->_frame_angles.back().at(2));

            //TODO (etsairi): In MATLAB code we have camera.rot_inv(ang - ang_prev), we need to check why code is different !

            // output rotation is from current frame to previous frame
            return camera.rot(ang_prev - ang, dist_corr->_frame_inputs.back().zoom_input);
        }
        else            // First matrix
        {
            IS_ASSERT(0); // Invalid SW flow
            return Hom::eye();
        }
    }

    inline Hom get_current_orientation(
        const StabilizerDistortionCorrection*   dist_corr,
        const Hom&                              rotation_from_curr_to_prev)
    {
        if (dist_corr->_frame_inputs.size() > 1) // if not first frame
        {
            const Camera& cam   = dist_corr->_camera[dist_corr->_frame_inputs.back().sensor_idx];
            const Hom K         = cam.to_mat();
            const Hom Ki        = cam.to_mat_inv();

            _camera_matrix_add_line(_debug, dist_corr->_frame_inputs.back().frame_id, K);

            _prev_orientation = _prev_orientation * Ki * rotation_from_curr_to_prev * K;

            return _prev_orientation.inv();
        }
        else
        {
            IS_ASSERT(0); // Invalid SW flow
           return Hom::eye();
        }
    }

    static inline HomographyWlsqSolver::Params read_dis_params(uint32_t min_matches)
    {
        HomographyWlsqSolver::Params params(min_matches);

#if defined(__DIS_LOAD_PARAMS__)
        double  gamma;
        double  beta;
        double  reg_weigth;
        double  exp_factor;

        FILE *pFile = NULL;
        errno_t err = 0;
        err = fopen_s(&pFile,"C://Users//maxima//Perforce//maxima_MAXIMA_3483//System//Src//EIS//Matlab//DIS//MatrixBased//dis_params.txt", "r");
        if (err==0 && pFile) {

            fscanf_s(pFile, "gamma=%lf\n", &gamma);
            fscanf_s(pFile, "beta=%lf\n", &beta);
            fscanf_s(pFile, "reg_weigth=%lf\n", &reg_weigth);
            fscanf_s(pFile, "exp_factor=%lf\n", &exp_factor);
            fclose(pFile);

            params._beta = beta;
            params._gamma = gamma;
            params._reg_weigth = reg_weigth;
            params._exp_factor = exp_factor;

            std::cout << "parameters read:\n";
            std::cout << "gamma=" << gamma << "\n";
            std::cout << "beta=" << beta << "\n";
            std::cout << "reg_weigth=" << reg_weigth << "\n";
            std::cout << "exp_factor=" << exp_factor << "\n";
        }
        else {
            std::cout << "failed to read parameters\n";
        }
#endif
        return params;
    }

public:

    DisStabilizer(
        const is_init_data_common*              init_common,
        const is_init_data_sensor*              init_sensors,
        const Camera*                           cameras,
        uint32_t                                num_sensors,
        uint32_t                                dis_buffer_delay,
        int32_t&                                status)
    :
        _params(init_sensors, num_sensors),
        _ransac(init_sensors, num_sensors, _min_matches, status),
        _solver(read_dis_params(_min_matches), cameras, num_sensors),
        _dis(MatrixBasedDis::Params(dis_buffer_delay),status),
        _bias_estimator(_max_curr_match_size, status),
        _prev_orientation(Hom::eye()),
        _curr_match_size(0)
    {
        IS_ASSERT(IS_RET_SUCCESS == status);

        (void)init_common;

#if defined(_EIS_INTERNAL_DEBUG_)
        _debug = new(std::nothrow) dis_debug_data(status);
        if (NULL == _debug)
        {
            IS_ERR(IS_ERR_OUT_OF_MEMORY, "failed to allocate dis_debug_data");
            status = IS_RET_OUT_OF_MEMORY;
        }
#endif /*_EIS_INTERNAL_DEBUG_  */
    }

    ~DisStabilizer()
    {
#if defined(_EIS_INTERNAL_DEBUG_)
        _DELETE(_debug);
#endif /* _EIS_INTERNAL_DEBUG_ */
    }

    inline void set_first_frame()
    {
        _ransac.set_first_frame();
        _solver.set_first_frame();

        _dis.set_first_frame();

        _prev_orientation = Hom::eye();
        _curr_match_size = 0;
    }

    inline void set_was_sensor_switch()
    {
        IS_ASSERT(0); // TODO (etsairi): check
    }

    inline bool calc_is_dis_enabled(
        const FilterInput&      frame_input_back,
        const is_input_dis_t*   in,
        bool                    is_first)
    {
        if (false == _params._is_sensor_enabled[frame_input_back.sensor_idx])
        {
            return false; // DIS is disabled for this sensor
        }

        if (frame_input_back.exposure_time >= _params._exposure_time_threshold_usec[frame_input_back.sensor_idx])
        {
            return false;
        }

        if (frame_input_back.focus_distance_cm < _params._focus_distance_threshold_cm[frame_input_back.sensor_idx])
        {
            return false;
        }

        if (false == in->alignment_matrix.enable)
        {
            IS_ASSERT(0); // This matrix should always be enabled, worts case confidence is low and matrix quality is garbage ...
            return false;
        }

        if (in->alignment_matrix.confidence <= _params._confidence_threshold[frame_input_back.sensor_idx])
        {
            return false;
        }

        if ((SceneDetection::SceneTypes::TRIPOD == frame_input_back.scene_type) ||
            (SceneDetection::SceneTypes::HANDHELD == frame_input_back.scene_type))
        {
            //TODO (etsairi): Consider the Hand-held case.
            //                On tripod there is no reason in the world that DIS should help, it can only introduce
            //                more undesired motion.
            //                In hand-held mode, we assume that OIS + EIS does not have a lot of mistakes, and since
            //                AF is not accurate enough in estimating the distance so it is safer to just disable DIS.
            return false;
        }

        if (false == is_valid_motion_vectors(in, frame_input_back.frame_id, is_first))
        {
            return false;
        }

        return true;
    }

    //TODO (etsairi): do we need to get the "is_first_frame" as input? what to do in sensor switch?
    inline int32_t update(
        const StabilizerDistortionCorrection*   dist_corr,
        const is_input_dis_t* const             dis_motion_vectors,
        uint64_t                                sof) // quaternion of this frame, or if using angles need to take
    {
        // The data structures are as follows:
        //  1. MVs are between this and previous frame
        //     That is, "tarCenterX" is in current frame domain and "refCenterX" is in previous frame domain.
        //  2. Rotation matrix is between this is previous frame
        //  3. Orientation matrix is between this and first frame. Where we define the first frame to have orientation zero.
        //  4. DIS transform is also between this and first frame (as it uses the orientations matrices)
        //
        // Therefore the follow is like this,
        //  1. For the first frame, init unity matrix to the DIS
        //  2. For all other frames,
        //  2.1.    Calculate rotations matrix
        //  2.2.    Using the rotation matrix and camera matrix calculate the orientation matrix
        //  2.3.    Using this frame rotation matrix (as it connects between this and previous frame, similarly to the MVs),
        //          frame input size (width and height, to calculate the scaling factor), zoom and camera matrix.
        //          Adjust the MVs to match the frame domain

        //TODO (etsairi): FIX zoom support !!!
//        IS_ASSERT(false == dist_corr->_frame_inputs.back().zoom_input.input.enable);
//        IS_ASSERT(false == dist_corr->_frame_inputs.back().zoom_input.output.enable);

        _dis_angle_add_line((false == dist_corr->_frame_angles.empty()), _debug, dist_corr->_frame_angles.back());

        IS_ASSERT(dist_corr->_frame_inputs.size() > 0);

        if (dist_corr->_frame_inputs.size() <= 1) // is first frame
        {
            _dis.set_first_matrix(Hom::eye(), dist_corr->_frame_inputs.back().frame_id); //TODO (etsairi): what to do in case of sensor switch?

            _orientation_add_line(_debug, dist_corr->_frame_inputs.back().frame_id, Hom::eye());
            return IS_RET_SUCCESS;
        }

        IS_ASSERT(dist_corr->_frame_angles.size() >= 2); // if we get here it means that this is not the first frame

        const FilterInput& frame_input_back = dist_corr->_frame_inputs.back();
        const Hom          rotation         = get_current_rotation(dist_corr);
        const Hom          orientation      = get_current_orientation(dist_corr, rotation);

        _frame_inputs_add_line(_debug, frame_input_back);

        // Runtime optimization:
        //  Calculate if DIS will be active for this frame based on scene settings.
        //  Note that DIS could still be disabled due to RANSAC
        //
        //  also checks if MVs are invalid, if true then DIS will be disabled for this frame
        const bool is_dis_enabled = calc_is_dis_enabled(frame_input_back, dis_motion_vectors, false);
        if (false == is_dis_enabled)
        {
            const Dis hom(Hom::eye(), false);
            const int32_t ret_dis = _dis.update(orientation, hom, frame_input_back.sensor_idx);
            if (IS_RET_SUCCESS != ret_dis)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "_dis.update() Failed");
                return ret_dis;
            }

            return IS_RET_SUCCESS;
        }

        _rotation_add_line(_debug, frame_input_back.frame_id, rotation);
        _orientation_add_line(_debug, frame_input_back.frame_id, orientation);

        // adjust motion vectors before RANSAC and DIS
        {
            const Camera&       camera  = dist_corr->_camera[frame_input_back.sensor_idx];
            const ZoomWindows&  zoom    = frame_input_back.zoom_input;
            construct_curr_match(dis_motion_vectors, rotation, camera, zoom);

            _dis_src_dst_add_line(_debug, _curr_match, _curr_match_size, frame_input_back.frame_id, 1);
        }

        // RANSAC to remove outliers
        {
            const uint32_t before_sz            = _curr_match_size;
            const EstimatorEssential2pt model   = _ransac.fit(_curr_match,
                                                              _curr_match_size,
                                                              frame_input_back.sensor_idx,
                                                              frame_input_back.frame_id);

            _dis_src_dst_add_line(_debug, _curr_match, _curr_match_size, frame_input_back.frame_id, 2);
            _ransac_model_add_line(_debug, model, _curr_match_size / (double)before_sz);

            _curr_match_size = Msac<Match2d, EstimatorEssential2pt>::filter_outliers(_curr_match, _curr_match_size);
            (void)model;
            (void)before_sz;

            _dis_src_dst_add_line(_debug, _curr_match, _curr_match_size, frame_input_back.frame_id, 0);
            _ransac_model_add_line(_debug, model, _curr_match_size / (double)before_sz);
        }

        int32_t ret = IS_RET_SUCCESS;

        // DIS
        {
            IS_ASSERT(true == is_dis_enabled); // Otherwise should have exited before

            const Dis hom = _solver.solve(frame_input_back.sensor_idx,
                                          _curr_match,
                                          _curr_match_size,
                                          frame_input_back.zoom_input,
                                          frame_input_back.frame_id);

            const int32_t ret_dis = _dis.update(orientation, hom, frame_input_back.sensor_idx);
            if (IS_RET_SUCCESS != ret_dis)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "_dis.update() Failed");
                ret = ret_dis;
            }

            _dis_raw_add_line(_debug, frame_input_back.frame_id, hom._mat);
        }

        // Bias estimation
        if (0)
        {
            const Camera& active_sensor_camera = dist_corr->_camera[frame_input_back.sensor_idx];

            // Adjust MVs so that bias estimation algorithm will work
            {
                // No need to adjust the MVs as they are already adjusted by the construct_curr_match() function.
                // Moreover, the call to _ransac.fit() cleared all the outliers

                const Hom K = active_sensor_camera.to_mat();
                const Hom Ki = active_sensor_camera.to_mat_inv();

                const Hom tmp = Ki * rotation * K;

                for (uint32_t idx = 0; idx < _curr_match_size; idx++)
                {
                    _curr_match[idx]._dst = tmp * _curr_match[idx]._dst;
                }
            }

            const Ang mid_ang = dist_corr->_frame_angles.back();

            // Sanity check
            IS_ASSERT(dist_corr->_frame_inputs.at_history(0).ts_start >
                      (dist_corr->_frame_inputs.at_history(1).ts_start + 100));

            // Delta is in microseconds
            const uint32_t dt = (uint32_t)(dist_corr->_frame_inputs.at_history(0).ts_start -
                                           dist_corr->_frame_inputs.at_history(1).ts_start);

            _bias_estimator.update(_curr_match, _curr_match_size, active_sensor_camera, mid_ang, dt, sof);
        }

        return ret;
    }

    inline int32_t update_in_termination_mode()
    {
        _dis.update_in_termination_mode();
        return IS_RET_SUCCESS;
    }

    //TODO (etsairi): not working for multi-sensor !!!
    inline int32_t process(
        const StabilizerDistortionCorrection*   dist_corr,
        DisStabilizeOutput&                     output)
    {
        //DIS part
        {
            uint32_t frame_idx;
            const Hom res = _dis.filter(frame_idx);
            if (true == _dis.has_valid_output())
            {
                const Camera& cam = dist_corr->_camera[dist_corr->_frame_inputs.at(0).sensor_idx];
                const Hom K = cam.to_mat();
                const Hom Ki = cam.to_mat_inv();

                output.has_output = true;
                output.dis_out2in = K*res.inv()*Ki; //TODO (etsairi): is this correct?
                output.frame_idx = frame_idx;

                _dis_filterd_add_line(_debug, dist_corr->_frame_inputs.back().frame_id, res);
                _dis_final_add_line(_debug, dist_corr->_frame_inputs.back().frame_id, output.dis_out2in);
            }
            else
            {
                output.has_output = false;
            }
        }

        // Bias estimation part
        {
            _bias_estimator.process();
        }

        return IS_RET_SUCCESS;
    }

    inline bool is_valid_motion_vectors(const is_input_dis_t* in, uint32_t frame_id, bool is_first);

};
#else
class DisStabilizer
{
private:
    FilterStateMachine      _state;
    uint32_t                _frame_idx;

public:
     DisStabilizer(
        const is_init_data_common*              init_common,
        const is_init_data_sensor*              init_sensors,
        const Camera*                           cameras,
        uint32_t                                num_sensors,
        uint32_t                                dis_support_size,
        int32_t&                                status)
     :
         _state(dis_support_size),
         _frame_idx(0)
     {
         (void)init_common;
         (void)init_sensors;
         (void)cameras;
         (void)num_sensors;
         (void)status;
     }

     ~DisStabilizer() {}

    inline void set_first_frame() {}

    inline void set_was_sensor_switch() {}

    inline int32_t update(
        const StabilizerDistortionCorrection*   dist_corr,
        const is_input_dis_t* const             dis_motion_vectors,
        uint64_t                                sof)
    {
        (void)dist_corr;
        (void)dis_motion_vectors;
        (void)sof;

        IS_ASSERT(dist_corr->_frame_inputs.size() > 0);

        if (dist_corr->_frame_inputs.size() <= 1) // is first frame
        {
            _frame_idx = dist_corr->_frame_inputs.back().frame_id; //TODO (etsairi): what to do in case of sensor switch?
        }

        return _state.update();
    }

    inline int32_t update_in_termination_mode()
    {
        return IS_RET_SUCCESS;
    }

    inline int32_t process(
        const StabilizerDistortionCorrection*   dist_corr,
        DisStabilizeOutput&                     output)
    {
        (void)dist_corr;
        (void)output;

        const FilterStateMachine::Action action = _state.set_action();

        const bool should_have_valid_output =
            ((FilterStateMachine::Action::RUN_CORE == action) ||
             (FilterStateMachine::Action::RUN_HISTORY == action));

        if (should_have_valid_output)
        {
            output.dis_out2in = Hom::eye();
            output.has_output = true;
            output.frame_idx = _frame_idx;
            _frame_idx++;
        }

        return IS_RET_SUCCESS;
    }

    inline bool is_valid_motion_vectors(const is_input_dis_t* in, uint32_t frame_id, bool is_first);

};
#endif /* __EIS_FL_ALGO__ */

inline bool DisStabilizer::is_valid_motion_vectors(const is_input_dis_t* in, uint32_t frame_id, bool is_first)
{
    (void)frame_id;

    if (NULL == in)
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: NULL pointer", frame_id);
        return false;
    }

    if (NULL == in->motion_vectors)
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: NULL pointer: in->motion_vectors", frame_id);
        return false;
    }

    if (0 == in->motion_vectors_buff_size_byte)
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: No motion vectors, motion_vectors_buff_size_byte is 0", frame_id);
        return false;
    }

    if (true == is_first)  //TODO (etsairi): is this too permissive?
    {
        // For the first frame MVs could be either empty or not ... depending on the CVP operation flow.
        // Either-way first frame MVs are ignored since MV contains matching between current and previous frame.
        // That is, "tarCenterX" is in current frame domain and "refCenterX" is in previous frame domain.
        return true;
    }

    const uint32_t mv_magic = HFI_CVP_VIDEO_STATS_GET_MAGIC(in->motion_vectors);
    if (CVP_MV_MAGIC_V100 != mv_magic)
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Invalid motion vectors buffer: 0x%8x, 0x%8x",
            frame_id, CVP_MV_MAGIC_V100, 
            mv_magic);
        return false;
    }

    if ((0 != (in->motion_vectors_buff_size_byte % 4)) ||
        (in->motion_vectors_buff_size_byte < sizeof(uint32_t) * 3) ||
        (0 != ((in->motion_vectors_buff_size_byte - sizeof(uint32_t) * 3)) % sizeof(HFI_CVP_MV_STATS_V2)))
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Invalid buffer size: %u [bytes]", frame_id, in->motion_vectors_buff_size_byte);
        return false;
    }

    const HFI_CVP_VIDEO_STATS_V2* mv = (HFI_CVP_VIDEO_STATS_V2*)in->motion_vectors;

    // Total size of
    const uint32_t num_expected_mv = ((in->motion_vectors_buff_size_byte / 4) - 3) / (sizeof(HFI_CVP_MV_STATS_V2) / 4);
    if (num_expected_mv != mv->nNumMv)
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Invalid number of motion vectors (expected, actual): %u, %u",
            frame_id, num_expected_mv, mv->nNumMv);
        return false;
    }

    if (num_expected_mv > HFI_DME_MAX_FPX)
    {
        IS_ERR(IS_ERR_INVALID_INPUT,
            "frame_id %u: Invalid number of motion vectors, exceeded expected maximal number of motion vectors (maximal value, actual): %u, %u",
            frame_id,
            HFI_DME_MAX_FPX,
            num_expected_mv);
        return false;
    }

    if (0 == num_expected_mv)
    {
        IS_ERR(IS_ERR_INVALID_INPUT,
            "frame_id %u: Invalid number of motion vectors, 0 motion vectors received", frame_id);
    }

    uint32_t err_cnt = 0;

    // Sanity check on transform value
    {
        IS_ASSERT(true == in->alignment_matrix.enable); // could be disabled?
        if (mv->modelType > 2)
        {
            IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Invalid matrix model type: %u", frame_id, mv->modelType);
            err_cnt++;
        }
    }

    // Sanity check on each of the vectors
    for (uint32_t idx = 0; idx < mv->nNumMv; idx++)
    {
        uint32_t tmp_cnt = 0;

        // Check vectors
        {
            //TODO (etsairi): add checks on vectors
        }

        // Check meta data rages
        {
            const int32_t score = CONVERT_SCORE_TO_INT(mv->sdisInfo[idx].meta.data.score); //TODO (etsairi): fix conversion
            if ((score < -4095) || (score > +4096))
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Invalid score value for MV #%u: %d, 0x%8x",
                    frame_id, idx, score, mv->sdisInfo[idx].meta.raw[0]);
                tmp_cnt++;
            }

            const int32_t robustness = CONVERT_ROBUSTNESS_TO_INT(mv->sdisInfo[idx].meta.data.robustness);  //TODO (etsairi): fix conversion
            if (robustness < -3 || robustness > 4)
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Invalid robustness value for MV #%u: %d, 0x%8x",
                    frame_id, idx, robustness, mv->sdisInfo[idx].meta.raw[0]);
                tmp_cnt++;
            }
        }

        err_cnt += (0 != tmp_cnt) ? 1 : 0;
    }

    if (err_cnt > 0)
    {
        IS_ERR(IS_ERR_INVALID_INPUT, "frame_id %u: Total number of MV's with errors: %u", frame_id, err_cnt);
    }

    return (0 == err_cnt) ? true : false;
}