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

#pragma once

#include "is_interface.h"
#include "eis_util.h"
#include "is_dbg_util.h"
#include "datatypes.h"
#include "filterstatemachine.h"
#include "biasestimatorsingleton.h"
#include "convolutionfilter.h"

//TODO (etsairi): organize debug defines
#if defined(_EIS_INTERNAL_DEBUG_)
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif /* _CRT_SECURE_NO_WARNINGS */
#include <stdio.h>
#endif /* _EIS_INTERNAL_DEBUG_ */

/*********************************************************************************************************************/
/*                  Definitions and Macros                                                                           */
/*********************************************************************************************************************/

#define EIS_MAX_FPS                     (60)
#define EIS_MIN_FPS                     (24)


#ifndef PROCESS_PREFIX_FILE_NAME
#define PROCESS_PREFIX_FILE_NAME                    IS_DBG_UTILS_DEFAULT_DIR"dump_data_sampler_process_"
#endif /* PROCESS_PREFIX_FILE_NAME */

#ifndef INTERPOLATED_PREFIX_FILE_NAME
#define INTERPOLATED_PREFIX_FILE_NAME               IS_DBG_UTILS_DEFAULT_DIR"dump_data_sampler_interpolated_"
#endif /* INTERPOLATED_PREFIX_FILE_NAME */

#ifndef OIS_INTEGRATED_FILE_NAME
#define OIS_INTEGRATED_FILE_NAME                    IS_DBG_UTILS_DEFAULT_DIR"dump_data_sampler_ois_integrated.csv"
#endif /* OIS_INTEGRATED_FILE_NAME */

#ifndef PROCESS_SCENE_DETECT_FILE_NAME
#define PROCESS_SCENE_DETECT_FILE_NAME              IS_DBG_UTILS_DEFAULT_DIR"dump_data_sampler_scene_detect_"
#endif /* PROCESS_SCENE_DETECT_FILE_NAME */

#ifndef GYRO_BUFFER_MAX_NUM_SEC
#define GYRO_BUFFER_MAX_NUM_SEC         (2)         /**< Number of seconds to buffer for gyro samplings */
#endif /* GYRO_BUFFER_MAX_NUM_SEC */

#ifndef PAUSE_RESUM_THRESHOLD_US
#define PAUSE_RESUM_THRESHOLD_US        ((uint32_t)SEC_TO_USEC(0.5))    /**< Threshold time in microseconds for pause resume use-case */
#endif /* PAUSE_RESUM_THRESHOLD_US */

#ifndef MAX_REQ_FREQUENCY
#define MAX_REQ_FREQUENCY              (1000U)      /**< Maximal requested frequency - 1 kilo Hz */
#endif /* MAX_REQ_FREQUENCY */

#ifndef MAX_FREQ_PERCENT_DEVIATION
#define MAX_FREQ_PERCENT_DEVIATION      (1.05)      /**< Maximal deviation from MAX_GYRO_FREQUENCY before overflow could occur */
#endif /* MAX_FREQ_PERCENT_DEVIATION */

#ifndef MAX_FREQ_SAFETY_MARGIN
#define MAX_FREQ_SAFETY_MARGIN          (1.25)      /**< Safety margins for samples buffer */
#endif /* MAX_FREQ_SAFETY_MARGIN */

#ifndef MIN_FREQ_SAFETY_MARGIN
#define MIN_FREQ_SAFETY_MARGIN          (0.8f)      /**< Safety margins for samples buffer */
#endif /* MIN_FREQ_SAFETY_MARGIN */

IS_COMPILE_TIME_ASSERT(MAX_FREQ_SAFETY_MARGIN > MAX_FREQ_PERCENT_DEVIATION);

#ifndef SAMPLES_EST_RATIO_ERROR_MARGIN_VS_AVG
#define SAMPLES_EST_RATIO_ERROR_MARGIN_VS_AVG      (0.01)     /**< Error margin of frequency estimation ratio w.r.t average sample rate */
#endif /* SAMPLES_EST_RATIO_ERROR_MARGIN_VS_AVG */

#ifndef SAMPLES_EST_RATIO_ERROR_MARGIN_VS_INIT
#define SAMPLES_EST_RATIO_ERROR_MARGIN_VS_INIT     (0.035)     /**< Error margin of frequency estimation ratio w.r.t requested sample rate */
#endif /* SAMPLES_EST_RATIO_ERROR_MARGIN_VS_INIT */

#ifndef GYRO_EST_MOVING_AVG_SUPPORT
#define GYRO_EST_MOVING_AVG_SUPPORT             (16)
#endif /* GYRO_EST_MOVING_AVG_SUPPORT */

#ifndef SAMPLES_EST_RATIO_ERROR_INFO_PERIOD
#define SAMPLES_EST_RATIO_ERROR_INFO_PERIOD    (64)    /**< If sample frequency estimation is different from requested frequency then
                                                        *   error will be outputted once every SAMPLES_EST_RATIO_ERROR_INFO_PERIOD frames.
                                                        *   Done in order to reduce the number of error produced by EIS algorithm.
                                                        */
#endif /* SAMPLES_EST_RATIO_ERROR_INFO_PERIOD */

#ifndef GYRO_NUM_FRAMES_TO_BUFFER
#define GYRO_NUM_FRAMES_TO_BUFFER                   (6) /**< Number of frames to be saved in the gyro samples buffer */
#endif /* GYRO_NUM_FRAMES_TO_BUFFER */

#ifndef IFOVC_NUM_FRAMES_TO_BUFFER
#define IFOVC_NUM_FRAMES_TO_BUFFER                  (6) /**< Number of frames to be saved in the iFOVC samples buffer */
#endif /* IFOVC_NUM_FRAMES_TO_BUFFER */

#ifndef IFOVC_MIN_SAMPLES_PER_FRAME
#define IFOVC_MIN_SAMPLES_PER_FRAME                 (2) /**< Minimal number of IFOVC samples per frame*/
#endif /* IFOVC_MIN_SAMPLES_PER_FRAME */

#ifndef IFOVC_MIN_SAMPLES_PER_FRAME_WHILE_CHANGE
#define IFOVC_MIN_SAMPLES_PER_FRAME_WHILE_CHANGE    (4) /**< Minimal number of IFOVC samples in a frame where IFOVC position changes */
#endif /* IFOVC_MIN_SAMPLES_PER_FRAME_WHILE_CHANGE */

#ifndef OIS_SHORT_FILTER_SAMPLES_SAFETY_MARGIN
#define OIS_SHORT_FILTER_SAMPLES_SAFETY_MARGIN      (1.2)
#endif /* OIS_SHORT_FILTER_SAMPLES_SAFETY_MARGIN */

#ifndef OIS_TRIPOD_SAMPLES_ADDITIONAL_MARGIN
#define OIS_TRIPOD_SAMPLES_ADDITIONAL_MARGIN        (1.2)
#endif /* OIS_TRIPOD_SAMPLES_ADDITIONAL_MARGIN */

#ifndef OIS_MAX_SHORT_FILTER_SUPPORT
#define OIS_MAX_SHORT_FILTER_SUPPORT                (128)
#endif /* OIS_MAX_SHORT_FILTER_SUPPORT */


/*********************************************************************************************************************/
/*                  Class Definitions                                                                                */
/*********************************************************************************************************************/

/*********************************************************************************************************************/
/*                  Sensors init classes                                                                             */
/*********************************************************************************************************************/

class SensorInitInfo
{
public:
    float                       _requested_frequency;
    float                       _noise_floor;
    const uint32_t              _num_axis;
    uint32_t                    _sensor_mount_angle;
    const CameraPosition        _camera_position;
    uint32_t                    _samples_ring_buffer_size;

protected:
    SensorInitInfo(
        uint32_t        num_axis,
        CameraPosition  camera_position)
    :
        _requested_frequency(0),
        _noise_floor(0),
        _num_axis(num_axis),
        _sensor_mount_angle(0),
        _camera_position(camera_position),
        _samples_ring_buffer_size(0)
    {}

public:
    SensorInitInfo(
        float           requested_frequency,
        float           noise_floor,
        uint32_t        num_axis,
        uint32_t        sensor_mount_angle,
        CameraPosition  camera_position)
    :
        _requested_frequency(requested_frequency),
        _noise_floor(noise_floor),
        _num_axis(num_axis),
        _sensor_mount_angle(sensor_mount_angle),
        _camera_position(camera_position),
        _samples_ring_buffer_size(0)
    {}

    ~SensorInitInfo() {}
};

class MultiSensorInitInfo : public SensorInitInfo
{
public:
    const is_init_data_common*  _init_common;
    const is_init_data_sensor*  _init_sensor;
    const uint32_t              _num_sensors;
    uint32_t                    _num_active_sensors;

public:
    virtual ~MultiSensorInitInfo() {}

    // Gets the relevant sensor frequency
    virtual uint32_t get_frequency(uint32_t sensor_idx) const = 0;

    // Gets the relevant sensor noise floor, if not defined then 0 will be returned.
    virtual float get_noise_floor(uint32_t sensor_idx) const
    {
        (void)sensor_idx;
        return 0;
    }

protected:
    MultiSensorInitInfo(
        uint32_t num_axis,
        const is_init_data_common*  init_common,
        const is_init_data_sensor*  init_sensors,
        uint32_t                    num_sensors)
    :
        SensorInitInfo(num_axis, init_sensors[0].camera_position),
        _init_common(init_common),
        _init_sensor(init_sensors),
        _num_sensors(num_sensors),
        _num_active_sensors(0)
    {}
};

class GyroInitInfo : public SensorInitInfo
{
public:
    GyroInitInfo(const is_init_data_sensor& sensor_0)
    :
        SensorInitInfo((float)sensor_0.is_chromatix_info.top.gyro_frequency,
                   sensor_0.is_chromatix_info.general.gyro_noise_floor,
                   3,
                   sensor_0.sensor_mount_angle,
                   sensor_0.camera_position)
    {
        const uint32_t samples_per_frame = (uint32_t)ceil((double)(_requested_frequency * MAX_FREQ_SAFETY_MARGIN) / EIS_MIN_FPS);
        const uint32_t num_frames_to_buffer = GYRO_NUM_FRAMES_TO_BUFFER;

        _samples_ring_buffer_size = MAX(128, num_frames_to_buffer * samples_per_frame);
    }

    ~GyroInitInfo() {}
};

class OISInitInfo : public MultiSensorInitInfo
{
public:
    OISInitInfo(
        const is_init_data_common*  init_common,
        const is_init_data_sensor*  init_sensors,
        uint32_t                    num_sensors)
    :
        MultiSensorInitInfo(2, init_common, init_sensors, num_sensors)
    {
        IS_ASSERT(num_sensors >= 1);

        _requested_frequency = 0;

        for (uint32_t idx = 0; idx < _num_sensors; idx++)
        {
            if (init_sensors[idx].is_chromatix_info.top.ois_frequency > 0)
            {
                _num_active_sensors++;
                _requested_frequency = MAX(_requested_frequency,
                                           (float)init_sensors[idx].is_chromatix_info.top.ois_frequency);

                //TODO: is this bug? overwrite _noise_floor and _sensor_mount_angle
                _noise_floor = init_sensors[idx].is_chromatix_info.general.ois_noise_floor;
                _sensor_mount_angle = init_sensors[idx].sensor_mount_angle;
            }
        }

        IS_ASSERT(_requested_frequency > 0); // Should not reach here, should crash on invalid input

        {
            const uint32_t samples_per_frame = (uint32_t)ceil((double)(_requested_frequency * MAX_FREQ_SAFETY_MARGIN) / EIS_MIN_FPS);
            const uint32_t samples_buffer_delay = samples_per_frame * init_common->buffer_delay;
            const uint32_t samples_support_for_short_filter = (uint32_t)ceil(OIS_MAX_SHORT_FILTER_SUPPORT * OIS_SHORT_FILTER_SAMPLES_SAFETY_MARGIN);

            _samples_ring_buffer_size = (uint32_t)MAX(1024, (samples_buffer_delay + samples_support_for_short_filter));
        }
    }

    virtual ~OISInitInfo() {};

    virtual uint32_t get_frequency(uint32_t sensor_idx) const
    {
        return _init_sensor[sensor_idx].is_chromatix_info.top.ois_frequency;
    }

    float get_noise_floor(uint32_t sensor_idx) const
    {
        return _init_sensor[sensor_idx].is_chromatix_info.general.ois_noise_floor;
    }

    SensorData::ois_module_type_t get_ois_type(uint32_t sensor_idx) const
    {
        return (SensorData::ois_module_type_t)_init_sensor[sensor_idx].is_chromatix_info.ois.module_type;
    }
};

class IFOVCInitInfo : public MultiSensorInitInfo
{
public:
    IFOVCInitInfo(
        const is_init_data_common*  init_common,
        const is_init_data_sensor*  init_sensors,
        uint32_t                    num_sensors)
    :
        MultiSensorInitInfo(2, init_common, init_sensors, num_sensors)
    {
        IS_ASSERT(num_sensors >= 1);

        _requested_frequency = 0;

        for (uint32_t idx = 0; idx < _num_sensors; idx++)
        {
            if (init_sensors[idx].is_chromatix_info.top.ifovc_frequency > 0)
            {
                _num_active_sensors++;
                _requested_frequency = MAX(_requested_frequency, (float)init_sensors[idx].is_chromatix_info.top.ifovc_frequency);

                _noise_floor = 0;
                _sensor_mount_angle = init_sensors[idx].sensor_mount_angle;
            }
        }

        IS_ASSERT(_requested_frequency > 0); // Should not reach here, should crash on invalid input

        const uint32_t samples_per_frame = (uint32_t)ceil((double)(_requested_frequency * MAX_FREQ_SAFETY_MARGIN) / EIS_MIN_FPS);
        const uint32_t num_frames_to_buffer = (IFOVC_NUM_FRAMES_TO_BUFFER + init_common->buffer_delay);

        _samples_ring_buffer_size = MAX(512, num_frames_to_buffer * samples_per_frame);
    }

    virtual ~IFOVCInitInfo() {}

    virtual uint32_t get_frequency(uint32_t sensor_idx) const
    {
        return _init_sensor[sensor_idx].is_chromatix_info.top.ifovc_frequency;
    }
};

/*********************************************************************************************************************/
/*                  Sample buffer classes                                                                            */
/*********************************************************************************************************************/

struct SamplerInterpolationStatistics
{
    uint32_t               num_processed;
    uint32_t               num_exrap_start;
    uint32_t               num_exrap_end;
    int32_t                num_error;
};

//TODO (etsairi): could be converted into to classes ? base + one that contains "RingBuffer<IdData>* _sensor_id"
template <typename T>
class MultiSensorDataBuffer
{
protected:
    class IdData
    {
    public:
        //TODO (etsairi): change to uint8_t and uint16_t
        uint32_t    _sensor_id;             /**< Sensor ID as appears in CamX */
        uint32_t    _sequence_size;         /**< Number of consecutive sensor ID entries */

        IdData(uint32_t sensor_id, uint32_t sequence_size) : _sensor_id(sensor_id), _sequence_size(sequence_size) {};

        ~IdData() {};
    };

private:
    static const uint32_t       _max_sampler_srt_length = 32;
    const uint32_t              _num_axis;

    //TODO (etsairi): make protected
public:
    RingBuffer<T>               _theta;         //TODO (etsairi): _theta -> _samples
    RingBuffer<IdData>*         _sensor_id;     //TODO (etsairi): _sensor_id ->

    char                        _sampler_str[_max_sampler_srt_length];

protected:
#if defined(_EIS_INTERNAL_DEBUG_)
    FILE*                       _debug_interpolated;
    FILE*                       _debug_process;
    FILE*                       _debug_scene_detect;
#endif /* _EIS_INTERNAL_DEBUG_ */

protected:    // static functions


    // Calculate linear interpolation in-place. Using the following, assume: x \in [x1, x2]
    // y = y1 + (x - x1) * (y2 - y1) / (x2 - x1)
    static void sample_data_interpolate(
        const sample_data_t&    start,
        const sample_data_t&    end,
        uint64_t                mid_point,
        Ang&                    out)
    {
        IS_ASSERT(start.ts < end.ts);

        IS_ASSERT(start.ts < mid_point);
        IS_ASSERT(end.ts > mid_point);

        // Sanity check - avoid devision by zero
        IS_ASSERT(((int32_t)(end.ts - start.ts)) > IS_EPS);

        // Sanity check - spacing between gyro samples should be less then ~35 minutes
        IS_ASSERT(end.ts <= (0x7FFFFFFF + start.ts));

        const uint32_t ts_delta = (uint32_t)(end.ts - start.ts);
        const uint32_t point_ts_delta = (uint32_t)(mid_point - start.ts);

        for (uint32_t idx = 0; idx < 3; idx++)
        {
            out(idx) = start.data[idx] + (point_ts_delta * (end.data[idx] - start.data[idx])) / ts_delta;
        }
    }

    static void sample_data_interpolate(
        const sample_quat_t&    start,
        const sample_quat_t&    end,
        uint64_t                mid_point,
        Quat&                   out)
    {
        IS_ASSERT(start.ts < end.ts);

        IS_ASSERT(start.ts < mid_point);
        IS_ASSERT(end.ts > mid_point);

        // Sanity check - avoid devision by zero
        IS_ASSERT(((int32_t)(end.ts - start.ts)) > IS_EPS);

        // Sanity check - spacing between gyro samples should be less then ~35 minutes
        IS_ASSERT(end.ts <= (0x7FFFFFFF + start.ts));

        const double ts_delta = (double)(end.ts - start.ts);
        const double point_ts_delta = (double)(mid_point - start.ts);
        const double w = point_ts_delta / ts_delta;

        out = Quat::slerp(start.quat, end.quat, w);
    }

    static void sample_data_interpolate(
        const sample_data_t&    start,
        const sample_data_t&    end,
        uint64_t                mid_point,
        Point2d&                out) //TODO (etsairi): replace with quaternion based integration
    {
        IS_ASSERT(start.ts < end.ts);

        IS_ASSERT(start.ts < mid_point);
        IS_ASSERT(end.ts > mid_point);

        // Sanity check - avoid devision by zero
        IS_ASSERT(((int32_t)(end.ts - start.ts)) > IS_EPS);

        // Sanity check - spacing between gyro samples should be less then ~35 minutes
        IS_ASSERT(end.ts <= (0x7FFFFFFF + start.ts));

        const uint32_t ts_delta = (uint32_t)(end.ts - start.ts);
        const uint32_t point_ts_data = (uint32_t)(mid_point - start.ts);

        {
            out.x = start.data[0] + (point_ts_data * (end.data[0] - start.data[0])) / ts_delta;
            out.y = start.data[1] + (point_ts_data * (end.data[1] - start.data[1])) / ts_delta;
        }
    }

    static void sample_data_interpolate(
        const sample_data_t&    start,
        const sample_data_t&    end,
        uint64_t                mid_point,
        double&                 out) //TODO (etsairi): replace with quaternion based integration
    {
        IS_ASSERT(start.ts < end.ts);

        IS_ASSERT(start.ts < mid_point);
        IS_ASSERT(end.ts > mid_point);

        // Sanity check - avoid devision by zero
        IS_ASSERT(((int32_t)(end.ts - start.ts)) > IS_EPS);

        // Sanity check - spacing between gyro samples should be less then ~35 minutes
        IS_ASSERT(end.ts <= (0x7FFFFFFF + start.ts));

        const uint32_t ts_delta = (uint32_t)(end.ts - start.ts);
        const uint32_t point_ts_data = (uint32_t)(mid_point - start.ts);

        out = start.data[0] + (point_ts_data * (end.data[0] - start.data[0])) / ts_delta;
    }

protected:
#if defined(_EIS_INTERNAL_DEBUG_)
    int32_t debug_data_init(const char* sampler_str)
    {
        errno_t err;

#if !defined(_WIN32) && !defined(_WIN64)
        IS_ERR("Compiling with internal EIS debug struct enabled");
#warning "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%s.csv", g_output_csv_base_folder, INTERPOLATED_PREFIX_FILE_NAME, sampler_str);

            err = IS_FOPEN(&(_debug_interpolated), output_csv, "w");
            if (err != 0)
            {
                return IS_RET_GENERAL_ERROR;
            }
        }

        {
            char output_csv[1024];
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s%s.csv", g_output_csv_base_folder, PROCESS_PREFIX_FILE_NAME, sampler_str);

            err = IS_FOPEN(&(_debug_process), output_csv, "w");
            if (err != 0)
            {
                return IS_RET_GENERAL_ERROR;
            }
        }

        {
            char output_csv[1024];
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%s%s.csv", g_output_csv_base_folder, PROCESS_SCENE_DETECT_FILE_NAME, sampler_str);

            err = IS_FOPEN(&(_debug_scene_detect), output_csv, "w");
            if (err != 0)
            {
                return IS_RET_GENERAL_ERROR;
            }
        }

        return IS_RET_SUCCESS;
    }
#endif /* _EIS_INTERNAL_DEBUG_ */

public:
    MultiSensorDataBuffer(
        uint32_t                    num_axis,
        uint32_t                    buffer_size,
        uint32_t                    num_sensors,
        char const*                 sampler_str,
        int32_t&                    status)
    :
        _num_axis(num_axis),
        _theta(buffer_size),
        _sensor_id(NULL)
    {
        IS_ASSERT(IS_RET_SUCCESS == status);

        if (0 == _theta.maxSize())
        {
            IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: constructor failed. Out of memory", "MultiSensorDataBuffer");
            status = IS_RET_OUT_OF_MEMORY;
            goto Exit;
        }

        IS_ASSERT(num_sensors > 0);

        if (num_sensors > 1)
        {
            _sensor_id =  new(std::nothrow) RingBuffer<IdData>(buffer_size);
            if (NULL == _sensor_id)
            {
                IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: constructor failed. Out of memory", "MultiSensorDataBuffer");
                status = IS_RET_OUT_OF_MEMORY;
                goto Exit;
            }
        }

        strlcpy(_sampler_str, sampler_str, _max_sampler_srt_length);

#if defined(_EIS_INTERNAL_DEBUG_)
        {
            int32_t tmp_status = debug_data_init(sampler_str);
            if (IS_RET_SUCCESS != tmp_status)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "%s: failed to init debug files", sampler_str);
                status = tmp_status;
                goto Exit;
            }
        }
#endif /* _EIS_INTERNAL_DEBUG_ */

    Exit:
        if (IS_RET_SUCCESS != status)
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: Constructor failed", "MultiSensorDataBuffer");
        }
    };

    ~MultiSensorDataBuffer()
    {
        _DELETE(_sensor_id);

#if defined(_EIS_INTERNAL_DEBUG_)
        IS_FCLOSE(_debug_interpolated);
        IS_FCLOSE(_debug_process);
        IS_FCLOSE(_debug_scene_detect);
#endif /* _EIS_INTERNAL_DEBUG_ */
    }

private:
    inline bool is_in_interval(uint64_t left_ts, uint64_t right_ts, uint64_t ts) const
    {
        return (left_ts <= ts) && (ts <= right_ts);
    }

protected:
    uint32_t find_idx_in_theta_binary_search(uint64_t ts, uint32_t left_idx, uint32_t right_idx) const
    {
        uint32_t mid = 0;
        uint32_t left = left_idx;
        uint32_t right = right_idx;

        IS_ASSERT(_theta.size() >= 1);

        if (_theta[0].ts >= ts)
        {
            return 0;
        }

        if (_theta[right].ts <= ts)
        {
            return right;
        }

        // Binary search
        while (left < right)
        {
            mid = (right + left) / 2;
            if (ts > _theta[mid].ts)
            {
                left = mid + 1;
            }
            else if (ts < _theta[mid].ts)
            {
                right = mid;
            }
            else
            {
                return mid;
            }
        }

        return mid;
    }

    void advance_indexes(uint32_t& left_idx, uint32_t& right_idx) const
    {
        IS_ASSERT(left_idx > 1);

        right_idx = left_idx - 1;
        left_idx = (uint32_t)MAX((int32_t)((int32_t)right_idx - (int32_t)_sensor_id->at(right_idx)._sequence_size + 1), 0);

        // Sanity checks
        IS_ASSERT((int32_t)right_idx > (int32_t)left_idx);
        IS_ASSERT(left_idx >= 0);
        IS_ASSERT(right_idx < _theta.size());
    }

    bool get_next_index(uint32_t sensor_id, uint32_t& left_idx, uint32_t& right_idx) const
    {
        if (0 == left_idx)
        {
            return false; // no next section
        }

        uint32_t tmp_left    = left_idx;
        uint32_t tmp_right   = right_idx;

        while (sensor_id != _sensor_id->at(tmp_right)._sensor_id)
        {
            advance_indexes(tmp_left, tmp_right);
        }

        if (sensor_id != _sensor_id->at(tmp_right)._sensor_id)
        {
            // If no next section return false and leave the pointers unchanged
            return false;
        }

        right_idx   = tmp_right;
        left_idx    = tmp_left;

        return true;
    }

    uint32_t find_idx_in_theta_with_sensor_id(uint64_t ts, uint32_t sensor_id) const
    {
        IS_ASSERT(NULL != _sensor_id);
        IS_ASSERT(_theta.size() == _sensor_id->size());

        // Check if buffer contains only data from a single sensor
        if (_sensor_id->at(_sensor_id->size() - 1)._sequence_size >= _sensor_id->size())
        {
            return find_idx_in_theta_binary_search(ts, 0, _theta.size() - 1);
        }

        // More than one sensor is stored in the data ring
        uint32_t right_idx = _theta.size() - 1;
        IS_ASSERT(right_idx + 1 >= _sensor_id->at(right_idx)._sequence_size); // Otherwise should not have gotten here ...

        uint32_t left_idx = right_idx - _sensor_id->at(right_idx)._sequence_size + 1;
        IS_ASSERT((int32_t)left_idx > 0); // Sanity check on internal structures

        IS_ASSERT(right_idx >= _sensor_id->back()._sequence_size);
        IS_ASSERT(_sensor_id->at(right_idx)._sensor_id == _sensor_id->at(left_idx)._sensor_id);

        bool was_found_first_feasible_interval = false;

        do
        {
            // Update left and right indexes
            {
                const bool ret = get_next_index(sensor_id, left_idx, right_idx);
                IS_ASSERT(true == ret); // what to do in case it was not found ?
                (void)ret;

                // found a possible interval, need to check that all conditions holds
                IS_ASSERT(sensor_id == _sensor_id->at(right_idx)._sensor_id);
                IS_ASSERT(sensor_id == _sensor_id->at(left_idx)._sensor_id);
            }

            if (true == is_in_interval(_theta.at(left_idx).ts, _theta.at(right_idx).ts, ts))
            {
                // Found the right interval, now find the correct index inside it
                return find_idx_in_theta_binary_search(ts, left_idx, right_idx);
            }

            // This is not the correct interval there could be two options:
            //  1. Could find another interval in the next iteration
            //  2. The point the was requested, ts, is outside of all intervals. So extrapolation is needed

            // Check if extrapolation is needed
            {
                // Extrapolation from the right
                if (_theta.at(right_idx).ts < ts)
                {
                    // Has to be the first interval with this sensor index otherwise theta ring
                    // timestamps are not monotonically increasing
                    IS_ASSERT(false == was_found_first_feasible_interval);
                    return right_idx;
                }

                if (left_idx == 0)
                {
                    // point is not in this interval and there is no other interval to look for ... return leftmost index
                    IS_ASSERT(false == is_in_interval(_theta.at(left_idx).ts, _theta.at(right_idx).ts, ts));
                    return left_idx;
                }

                uint32_t next_left_idx = left_idx;
                uint32_t next_right_idx = right_idx;
                advance_indexes(next_left_idx, next_right_idx);

                // Extrapolation from the left
                if (false == get_next_index(sensor_id, next_left_idx, next_right_idx))
                {
                    // No other interval with relevant sensor_id, need to extrapolate from the left
                    return left_idx;
                }
                else
                {
                    // Check for option 2, if the point the was requested between the two found intervals.
                    // That is, it is not inside none of them. Otherwise it would have been found in this or in the
                    // previous iteration.

                    // Not in this iteration interval
                    IS_ASSERT(false == is_in_interval(_theta.at(left_idx).ts, _theta.at(right_idx).ts, ts));

                    // Not in previous iteration interval
                    IS_ASSERT((0 == next_left_idx) ||
                              (false == is_in_interval(_theta.at(next_left_idx).ts, _theta.at(next_right_idx).ts, ts)));

                    // Since we are scanning from the end to the start
                    IS_ASSERT(_theta.at(left_idx).ts > _theta.at(next_right_idx).ts);

                    //Sanity on theta
                    IS_ASSERT(_theta.at(next_left_idx).ts < _theta.at(next_right_idx).ts);
                    IS_ASSERT(_theta.at(left_idx).ts < _theta.at(right_idx).ts);

                    if ((_theta.at(next_right_idx).ts < ts) && (ts < _theta.at(left_idx).ts))
                    {
                        // we need to extrapolate, since ts is missing from theta.
                        // Take closest index

                        const uint64_t delta_from_right = (uint64_t)(ts - _theta.at(next_right_idx).ts);
                        const uint64_t delta_from_left = (uint64_t)(_theta.at(left_idx).ts - ts);

                        if (delta_from_right < delta_from_left)
                        {
                            // closest to the right point
                            return next_right_idx;
                        }
                        else
                        {
                            // closest to the left point
                            return left_idx;
                        }
                    }
                }
            }

            was_found_first_feasible_interval = true;
            advance_indexes(left_idx, right_idx);
        } while (left_idx > 0);

        IS_ASSERT(true == was_found_first_feasible_interval);
        IS_ASSERT(0); // Should not reach here ...
        return _theta.size() - 1;
    }

public:
    bool ts_delta_too_big(uint64_t left_ts, uint64_t right_ts) const
    {
        IS_ASSERT(left_ts < right_ts);

        if (right_ts > left_ts)
        {
            return ((int64_t)(right_ts - left_ts) > MILLI_TO_USEC(200));
        }
        else
        {
            return false;
        }
    }

    bool was_sensor_changed(uint32_t theta_idx) const
    {
        if (NULL == _sensor_id)
        {
            return false; // Working in single sensor mode
        }
        else
        {
            if ((0 == theta_idx) || (theta_idx >= (_sensor_id->size() - 1)))
            {
                return false;  // First or last samples
            }
            else
            {
                // returns true if sensor was changed w.r.t. previous frame. Otherwise false
                return (_sensor_id->at(theta_idx)._sensor_id != _sensor_id->at(theta_idx - 1)._sensor_id);
            }
        }
    }

    void push_data(const sample_data_t& data, uint32_t active_sensor_idx)
    {
        _theta.push(data);

        if (NULL != _sensor_id)
        {
            if ((true == _sensor_id->empty()) || (_sensor_id->back()._sensor_id != active_sensor_idx))
            {
                _sensor_id->push(IdData(active_sensor_idx, 1));
            }
            else
            {
                _sensor_id->push(IdData(active_sensor_idx, MIN(_sensor_id->maxSize(), _sensor_id->back()._sequence_size + 1)));
            }
        }
    }

    uint32_t find_idx_in_theta(uint64_t ts, uint32_t sensor_id) const
    {
        if (NULL == _sensor_id)
        {
            return find_idx_in_theta_binary_search(ts, 0, _theta.size() -1);
        }
        else
        {
            return find_idx_in_theta_with_sensor_id(ts, sensor_id);
        }
    }

    uint32_t find_end_idx_in_theta(uint64_t ts, uint32_t sensor_id, uint32_t start_idx) const
    {
        uint32_t end_idx = start_idx;

        if (NULL == _sensor_id)
        {
            do
            {
                end_idx++;
            } while ((end_idx < _theta.size()) && (_theta[end_idx].ts < ts));
        }
        else
        {
            do
            {
                end_idx++;
            } while ((end_idx < _theta.size()) && (_theta[end_idx].ts < ts) && (sensor_id == _sensor_id->at(end_idx)._sensor_id));

            if ((end_idx > 0) && ((end_idx == _theta.size()) || (sensor_id != _sensor_id->at(end_idx)._sensor_id)))
            {
                end_idx--;
            }
        }

        IS_ASSERT(end_idx <= _theta.size());
        if ((end_idx > 0) && ((end_idx == _theta.size()) ||
            (true == ts_delta_too_big(_theta[end_idx - 1].ts, _theta[end_idx].ts)))) // truncate the last sample if it belongs to a different section
        {
            end_idx--;
        }

        return end_idx;
    }

    void set_first_frame()
    {
        _theta.clear();

        if (NULL != _sensor_id)
        {
            _sensor_id->clear();
        }
    }

    template <typename _T, typename _S>
    inline void set_out_by_theta(_T& out, const RingBuffer<_S>& theta, uint32_t theta_idx) const
    {
        for (uint32_t axis_idx = 0; axis_idx < _num_axis; axis_idx++)
        {
            out.at(axis_idx) = theta[theta_idx].data[axis_idx];    // take closest angle
        }
    }

    inline void set_out_by_theta(Quat& out, const RingBuffer<sample_quat_t>& theta, uint32_t theta_idx) const
    {
        out = theta[theta_idx].quat;
    }

    // inputs: array of timestamps and number of elements, output buffer is allocated by the caller
    // assumption: timestamps array is sorted
    // output: fills output angles interpolated according to the given array
    //         output array will be in the following format:
    //              delta     := 1 / (num_points - 3)
    //              delta_ts  := ts_end - ts_start
    //
    //              out[0].ts               = ts_start - (delta_ts * delta)
    //              out[1].ts               = ts_start
    //              out[2,..,num_points -3] = ts_start + (delta_ts * (idx - 1) * delta)
    //              out[num_points -2].ts   = ts_end
    //              out[num_points -1].ts   = ts_end   + (delta_ts * delta)
    // Return: 0 in case all points were interpolated\extrapolated as expected.
    //         In case more than one point was missing from each end than the function will fail and return the number of extrapolated points
    template <typename _T>
    void get_interp(
        uint64_t                        ts_start,
        uint64_t                        ts_end,
        uint32_t                        num_points,
        uint32_t                        sensor_id,
        uint32_t                        frame_id,
        bool                            is_first_frame,
        _T*                             out,
        SamplerInterpolationStatistics& stats,
        bool                            write_logs_to_file = true,
        bool                            is_ifovc_60_fps = false) const
    {
        (void)frame_id;
        (void)is_first_frame;
        (void)write_logs_to_file;

        memset(&stats, 0, sizeof(stats));

        for (uint32_t idx = 0; idx < num_points; idx++)
        {
            // Calculate timestamps to be interpolated\extrapolated around
            const uint64_t ts = timestamp_interpolation(ts_start, ts_end, num_points, idx);

            // Find timestamp in theta ring
            const uint32_t theta_idx = find_idx_in_theta(ts, sensor_id);

            // get first below \ above timestamps
            // if first or last point than it OK to extrapolate, otherwise wrong interval was received
            bool need_extrapolate = false;
            uint32_t below_idx = theta_idx;
            uint32_t above_idx = theta_idx;

            if (_theta[below_idx].ts > ts)
            {
                // in case of multi-sensor mode when only one sensor is active there could be huge gapes in the data
                // For example, assume multi-camera with a single OIS sensor and perform the following:
                //      1. start from sensor with OIS
                //      2. switch to sensor without OIS for several seconds
                //      3. Return to the sensor with OIS
                // If we perform linear interpolation between the last samples of 1) and the first of 3) it could
                // yield incorrect values.

                IS_ASSERT(theta_idx < _theta.size());
                IS_ASSERT(theta_idx >= 0);

                if ((0 == below_idx) ||                             // first sample
                    (true == was_sensor_changed(below_idx)) ||      // multiple sensors and this is the first sample in this section
                    (true == ts_delta_too_big(_theta[below_idx - 1].ts, _theta[below_idx].ts))) // delta between consecutive samples is too big (more than several frames)
                {
                    stats.num_exrap_start++;
                    need_extrapolate = true;

                    if (idx > 0)
                    {
                        stats.num_error++;
                    }
                }
                else
                {
                    below_idx--;
                }
            }
            else if (_theta[above_idx].ts < ts)
            {
                if (((_theta.size() - 1) == above_idx) ||               // last sample
                    (true == was_sensor_changed(above_idx + 1)) ||      // multiple sensors and this is the last sample in this section
                    (true == ts_delta_too_big(_theta[above_idx].ts, _theta[above_idx + 1].ts))) // delta between consecutive samples is too big (more than several frames)
                {
                    stats.num_exrap_end++;
                    need_extrapolate = true;

                    if (idx < (num_points - 1))
                    {
                        stats.num_error++;
                    }
                }
                else
                {
                    above_idx++;
                }
            }
            else
            {
                IS_ASSERT(ts == _theta[theta_idx].ts);
            }

            IS_ASSERT(below_idx <= above_idx);
            IS_ASSERT((below_idx == theta_idx) || (above_idx == theta_idx));

            if (false == need_extrapolate)
            {
                if (below_idx != above_idx)
                {
                    IS_ASSERT(_theta[below_idx].ts <= ts);
                    IS_ASSERT(_theta[above_idx].ts >= ts);

                    sample_data_interpolate(_theta[below_idx], _theta[above_idx], ts, out[idx]);
                }
                else
                {
                    set_out_by_theta(out[idx], _theta, theta_idx);
                }
            }
            else
            {
                if (idx > 0)
                {
                    out[idx] = out[idx - 1]; // Duplicate angles from the end
                }
                else
                {
                    set_out_by_theta(out[0], _theta, below_idx);

                    stats.num_error++;
                    // Should not have missed data from the past (unless first frame)
                }
            }

            stats.num_processed++;

#if defined(_EIS_INTERNAL_DEBUG_)
            if (true == write_logs_to_file)
            {
                write_get_interp_logs_to_file(out[idx], frame_id, idx, ts, need_extrapolate);
            }
#endif /* _EIS_INTERNAL_DEBUG_ */
        }

        if (false == is_ifovc_60_fps) // ifovc on 60 FPS yields replicated data for every second frame. This flag will override the error printing in that case.
        {
            if (stats.num_error > 0)
            {
                if (true == is_first_frame)
                {
                    IS_ERR(IS_ERR_SAMPLER_FIRST_MISSING_SAMPLE,
                        "%s: frame #%u: Too many points extrapolated from first frame (num_processed, num_exrap_start, num_exrap_end, num_error): %u, %u, %u, %d",
                        _sampler_str, frame_id, stats.num_processed, stats.num_exrap_start, stats.num_exrap_end, stats.num_error);
                }
                else
                {
                    if (stats.num_error > 25)
                    {
                        IS_ERR(IS_ERR_SAMPLER_MISSING_MANY_SAMPLES, "%s: frame #%u: All samples are missing from frame!", _sampler_str, frame_id);
                    }

                    IS_ERR(IS_ERR_SAMPLER_MISSING_MANY_SAMPLES,
                        "%s: frame #%u: Too many points extrapolated (num_processed, num_exrap_start, num_exrap_end, num_error): %u, %u, %u, %d",
                        _sampler_str, frame_id, stats.num_processed, stats.num_exrap_start, stats.num_exrap_end, stats.num_error);
                }
            }
            else
            {
                if ((stats.num_exrap_start + stats.num_exrap_end) > 0)
                {
                    IS_HIGH(IS_ERR_SAMPLER_LAST_MISSING_SAMPLE,
                        "%s: frame #%u: Points were extrapolated (num_processed, num_exrap_start, num_exrap_end, num_error): %u, %u, %u, %d",
                        _sampler_str, frame_id, stats.num_processed, stats.num_exrap_start, stats.num_exrap_end, stats.num_error);
                }
            }
        }
    }

#if defined(_EIS_INTERNAL_DEBUG_)

    template <typename _T>
    void write_vec_get_interp_logs_to_file(const _T& out, uint32_t frame_id, uint32_t idx, uint64_t ts, bool need_extrapolate) const
    {
        IS_ASSERT(NULL != _debug_interpolated);
        fprintf(_debug_interpolated, "%u, %u, %" PRIu64 ",", frame_id, idx, ts);

        for (uint32_t axis_idx = 0; axis_idx < _num_axis; axis_idx++)
        {
            fprintf(_debug_interpolated, "%5.15lf, ", out.at(axis_idx));
        }

        fprintf(_debug_interpolated, "%u\n", (uint32_t)need_extrapolate);
    }

    template <typename _T>
    void write_get_interp_logs_to_file(const _T& out, uint32_t frame_id, uint32_t idx, uint64_t ts, bool need_extrapolate) const
    {
        write_vec_get_interp_logs_to_file(out, frame_id, idx, ts, need_extrapolate);
    }

    void write_get_interp_logs_to_file(const Quat& out, uint32_t frame_id, uint32_t idx, uint64_t ts, bool need_extrapolate) const
    {
        const Ang angle = out.to_euler();;
        write_vec_get_interp_logs_to_file(angle, frame_id, idx, ts, need_extrapolate);
    }
#endif /* _EIS_INTERNAL_DEBUG_ */
};

/*********************************************************************************************************************/
/*                  Sensor Data handler classes                                                                      */
/*********************************************************************************************************************/

class SensorDataHandler
{
private:    // static functions
//    typedef void(rotate_sample)(sample_data_t&);  //TODO (etsairi): replace with: typedef sample_data_t&(rotate_sample)(const sample_data_t&)

    static void do_nothing(sample_data_t& sample) { (void)sample; }

    //static void rotate_general(sample_data_t& sample, int axis_x, int axis_y, int axis_z)
    //{
    //    IS_ASSERT(ABS(axis_x) != ABS(axis_y));
    //    IS_ASSERT(ABS(axis_y) != ABS(axis_z));
    //    IS_ASSERT(ABS(axis_x) != ABS(axis_z));
    //    IS_ASSERT(ABS(axis_x) <= 3);
    //    IS_ASSERT(ABS(axis_y) <= 3);
    //    IS_ASSERT(ABS(axis_z) <= 3);
    //    IS_ASSERT(ABS(axis_z) >= 1);
    //

    //    sample_data_t tmp_sample = sample;
    //    sample.data[0] = axis_x > 0 ? tmp_sample.data[ABS(axis_x) - 1] : -tmp_sample.data[ABS(axis_x) - 1];
    //    sample.data[1] = axis_y > 0 ? tmp_sample.data[ABS(axis_y) - 1] : -tmp_sample.data[ABS(axis_y) - 1];
    //    sample.data[2] = axis_z > 0 ? tmp_sample.data[ABS(axis_z) - 1] : -tmp_sample.data[ABS(axis_z) - 1];
    //}

    // Code was generated using generate_rotate_func.m
    static void rotate_p1_p2_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_p1_p2_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_p1_n2_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_p1_n2_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_n1_p2_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_n1_p2_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_n1_n2_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_n1_n2_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_p1_p3_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_p1_p3_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_p1_n3_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_p1_n3_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_n1_p3_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_n1_p3_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_n1_n3_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_n1_n3_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[0]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_p2_p1_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_p2_p1_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_p2_n1_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_p2_n1_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_n2_p1_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_n2_p1_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_n2_n1_p3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[2]; }
    static void rotate_n2_n1_n3(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[2]; }
    static void rotate_p2_p3_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_p2_p3_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_p2_n3_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_p2_n3_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_n2_p3_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_n2_p3_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = 1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_n2_n3_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_n2_n3_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[1]; sample.data[1] = -1 * tmp_sample.data[2]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_p3_p1_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_p3_p1_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_p3_n1_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_p3_n1_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_n3_p1_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_n3_p1_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_n3_n1_p2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = 1 * tmp_sample.data[1]; }
    static void rotate_n3_n1_n2(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[0]; sample.data[2] = -1 * tmp_sample.data[1]; }
    static void rotate_p3_p2_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_p3_p2_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_p3_n2_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_p3_n2_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = 1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_n3_p2_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_n3_p2_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = 1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[0]; }
    static void rotate_n3_n2_p1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = 1 * tmp_sample.data[0]; }
    static void rotate_n3_n2_n1(sample_data_t& sample) { const sample_data_t tmp_sample = sample; sample.data[0] = -1 * tmp_sample.data[2]; sample.data[1] = -1 * tmp_sample.data[1]; sample.data[2] = -1 * tmp_sample.data[0]; }

    static void front_0(sample_data_t& sample)
    {
        sample.data[0] = -sample.data[0];
        sample.data[1] = -sample.data[1];
    }

    static void front_90(sample_data_t& sample)
    {
        const double temp = sample.data[0];
        sample.data[0] = -sample.data[1];
        sample.data[1] = temp;
    }

    static void front_180(sample_data_t& sample)
    {
        (void)sample;
        return;
    }

    static void front_270(sample_data_t& sample)
    {
        const double temp = sample.data[0];
        sample.data[0] = sample.data[1];
        sample.data[1] = -temp;
    }

    static void rear_0(sample_data_t& sample)
    {
        sample.data[0] = -sample.data[0];
        sample.data[2] = -sample.data[2];
    }

    static void rear_90(sample_data_t& sample)
    {
        const double temp = sample.data[0];
        sample.data[0] = -sample.data[1];
        sample.data[1] = -temp;
        sample.data[2] = -sample.data[2];
    }

    static void rear_180(sample_data_t& sample)
    {
        sample.data[1] = -sample.data[1];
        sample.data[2] = -sample.data[2];
    }

    static void rear_270(sample_data_t& sample)
    {
        const double temp = sample.data[0];
        sample.data[0] = sample.data[1];
        sample.data[1] = temp;
        sample.data[2] = -sample.data[2];
    }

protected: // functions
    float estimate_frequency(const samples_data_t* samples) const
    {
        if (samples->num_elements == 0)
        {
            IS_LOW(IS_VRB_DEBUG_INFO, "%s: estimate_frequency() was called with num_elements == 0", _sampler_str);
            return 0;
        }

        uint64_t total_time_diff = 0;
        uint32_t samples_num_count = 0;

        for (uint32_t ii = 1; ii < samples->num_elements; ii++)
        {
            if (samples->samples[ii].ts > samples->samples[ii - 1].ts)
            {
                const uint64_t time_diff = samples->samples[ii].ts - samples->samples[ii - 1].ts;
                total_time_diff += time_diff;
                samples_num_count++;
            }
        }

        if (0 == total_time_diff)
        {
            IS_ERR(IS_ERR_INVALID_INPUT,
                "%s: Not enough samples to estimate frequency "
                "(num samples, first sample ts, last sample ts): (%u, %" PRIu64 ", %" PRIu64 ")",
                _sampler_str, samples->num_elements, samples->samples[0].ts, samples->samples[samples->num_elements - 1].ts);
            return 0; // Avoid devision by 0
        }

        return (samples_num_count * 1000000.0f) / total_time_diff;
    }

    void is_valid_frequency(const samples_data_t* samples, uint32_t frame_id) const
    {
        // Estimate the freq. in Hz according to this frame samples input
        const float estimated_frequency = estimate_frequency(samples);

        // Make sure that estimated freq. is not larger than maximal supported freq. (could cause buffer overflow).
        if (estimated_frequency > (_max_frequency * MAX_FREQ_PERCENT_DEVIATION))
        {
            IS_ERR(IS_ERR_SAMPLE_RATE_EXCEEDS,
                "%s: frame_id %u: estimated_frequency = %.1fHz is too high comparing to max_frequency = %.1fHz!",
                _sampler_str, frame_id, estimated_frequency, (_max_frequency * MAX_FREQ_PERCENT_DEVIATION));
        }

        // Check that estimated freq. for current frame is close to the average estimated freq. (could indicate on missing frames).
        {
            const float gyro_freq_hz_error_ratio = ABS((float)_estimated_frequency - (float)(estimated_frequency))
                / _estimated_frequency;

            if (SAMPLES_EST_RATIO_ERROR_MARGIN_VS_AVG < gyro_freq_hz_error_ratio) // samples rate is different between frames
            {
                IS_ERR(IS_ERR_SAMPLE_RATE_EST_VS_AVG,
                    "%s: frame_id %u: estimated_frequency = %.1fHz is different between frames! (might indicate on missing samples / false time stamps) average estimated freq = %.1fHz",
                    _sampler_str, frame_id, estimated_frequency, _estimated_frequency);
            }
        }

        // Check that average estimated gyro freq. does not deviate from requested freq. (could indicate on invalid input).
        if (0 == (frame_id % SAMPLES_EST_RATIO_ERROR_INFO_PERIOD))  //TODO (etsairi): preform the check for each frame but print only periodically
        {
            const float freq_hz_error_ratio = ABS(_estimated_frequency - _requested_frequency) / _requested_frequency;

            if (SAMPLES_EST_RATIO_ERROR_MARGIN_VS_INIT < freq_hz_error_ratio) // make sure estimated sample rate is not too different from requested sample rate
            {
                IS_ERR(IS_ERR_SAMPLE_RATE_EST_VS_INIT,
                    "%s: frame_id %u: Average estimated frequency = %.1fHz is different from requested frequency = %.1fHz",
                    _sampler_str, frame_id, _estimated_frequency, _requested_frequency);
            }
        }
    }

    bool is_valid_samples_spacings(const samples_data_t* samples, uint32_t frame_id) const
    {
        (void)frame_id;
        // Delta in micro seconds between consecutive gyro samples
        const float samples_delta_est_us = (1000000.0f / _estimated_frequency);

        // Check this is not first frame and not pause and not resume just happened
        {
            if (_last_sample.ts > 0) // Not the first frame
            {
                if (samples->samples[0].ts <= _last_sample.ts)
                {
                    // No gap between the samples, if samples are missing at the middle of the frame it
                    // will be notified by is_valid_frequency()
                    return true;
                }

                IS_ASSERT(samples->samples[0].ts > _last_sample.ts);

                if (samples->samples[0].ts >= (PAUSE_RESUM_THRESHOLD_US + _last_sample.ts))
                {
                    // Pause-resume case, no reason to notify the user on gap in the samples (as it is expected)
                    return true;
                }

                const float ts_delta = (float)((uint32_t)((int64_t)samples->samples[0].ts - (int64_t)_last_sample.ts));

                IS_ASSERT(ts_delta > 0);
                IS_ASSERT(ts_delta < (float)PAUSE_RESUM_THRESHOLD_US);

                const float max_delta_warn = samples_delta_est_us * (1 + 1) * 1.05f; // lost 1 samples time + additional 5% tolerance
                const float max_delta_error = samples_delta_est_us * (1 + 3) * 1.05f; // lost 3 samples time + additional 5% tolerance

                if (ts_delta > max_delta_error)
                {
                    IS_ERR(IS_ERR_SAMPLER_MISSING_MANY_SAMPLES, "%s: frame_id %u: samples->samples[0].ts - _last_sample.ts = %f[us] > %f[us]",
                        _sampler_str, frame_id, ts_delta, max_delta_error);

                    if (false == _relaxed_last_requested_interval_check)
                    {
                        return false;
                    }
                }
                else if (ts_delta > max_delta_warn)
                {
                    IS_ERR(IS_ERR_SAMPLER_MISSING_SAMPLE, "%s: frame_id %u: samples->samples[0].ts - _last_sample.ts = %f[us] > %f[us]",
                        _sampler_str, frame_id, ts_delta, max_delta_warn);
                }
            }
        }

        return true;
    }

    bool is_valid_frame_time(const samples_data_t* samples, uint32_t frame_id) const
    {
        (void)frame_id;
        // Delta in micro seconds between consecutive gyro samples
        const float sample_time_us = (1000000.0f / _estimated_frequency);

        // Case for timestamps are zeros. got that when doing sensor switch on Oppo. Worth checking.
        if (samples->samples[samples->num_elements - 1].ts < (uint32_t)sample_time_us)
        {
            IS_ERR(IS_ERR_SAMPLE_OOO,
                "%s: frame_id %u: received timestamp (%" PRIu64 ") is invalid",
                _sampler_str, frame_id, samples->samples[samples->num_elements - 1].ts);

            return false;
        }

        // Denote:
        //  req_first_gyro_ts       := info->last_requested.first_ts
        //  req_last_gyro_ts        := info->last_requested.last_ts
        //  actual_first_gyro_ts    := samples->gyro_data[0].ts
        //  actual_last_gyro_ts     := samples->gyro_data[samples->num_elements - 1].ts
        //
        // The correct order must hold the following:
        //      req_first_ts <= actual_first_ts <= (req_first_ts + sample_time_us) < (req_first_ts + 1.2 *sample_time_us)
        //
        //                   (1)                (2)                              (3)
        //
        //   <  (req_last_ts - sample_time_us) <= actual_last_ts < req_last_ts
        //  (4)                                 (5)              (6)
        //
        //  Notes:
        //      - If (1) and (6) doesn't hold it means that more data was supplied which will not trigger a
        //        warning only info message will be printed, since it might indicate on issue with the requesting mechanism
        //      - If (2) doesn't hold, that is,
        //              actual_first_ts \not in [req_first_ts, (req_first_ts + sample_time_us))
        //        Warning will be trigger only in case _relaxed_last_requested_interval_check is false
        //      - If (3) doesn't hold, that is,
        //              actual_first_ts \not in [req_first_ts, (req_first_ts + 1.2 * sample_time_us))
        //        Warning will be triggered either-way
        //      - If (4) doesn't always hold for sensor switch cases when sof diff can be very small even slightly negative at times
        //
        //      - If (5) doesn't hold, that is,
        //              actual_last_ts \not in [(req_last_ts - sample_time_us), req_last_ts)
        //        Warning will be triggered
        //      - If the following doesn't hold,
        //           (sample_time_us <= 100)
        //        Assertion will be triggered, since gyro frequency is bounded by 10,000 Hz

        const uint64_t req_first_ts     = _last_requested_interval.first_ts;
        const uint64_t req_last_ts      = _last_requested_interval.last_ts;
        const uint64_t actual_first_ts  = samples->samples[0].ts;
        const uint64_t actual_last_ts   = samples->samples[samples->num_elements - 1].ts;

        // Output of eis_internal_get_gyro_samples_time_interval(). Assumed to be correct
        IS_ASSERT(req_first_ts < req_last_ts);
        IS_ASSERT(samples->samples[samples->num_elements - 1].ts >= (uint32_t)sample_time_us);

        // Make sure that there are not missing samples from the beginning of the frame, since some of the samples
        // could have been retrieved in previous frame validate it against "last_gyro_sample" timestamp.

        // (2),(3) Should hold : actual_first_ts <= (req_first_ts + sample_time_us) < (req_first_ts + 1.2 *sample_time_us)
        //  Make sure that there are not missing samples at the start of the frame.

        // For OIS we request (due to issues with data request): ois_req_interval->first_ts = MIN(ois_last_ts, gyro_req_interval->first_ts) - (uint32_t)(2U * ois_sample_time_usec);
        // Thus don't need to error according to req_first_ts but to MAX(req_first_ts, _last_sample.ts)
        const uint64_t req_first_ts_effective = (true == _relaxed_last_requested_interval_check) ? MAX(req_first_ts, _last_sample.ts) : req_first_ts;
        if (actual_first_ts > req_first_ts_effective + 1.2 * sample_time_us)
        {
            IS_ERR(IS_ERR_SAMPLER_FIRST_MISSING_SAMPLE,
                "%s: frame_id %u: First requested timestamp (%" PRIu64 ") is smaller than received timestamp (%" PRIu64 "). Delta [usec] = %u",
                _sampler_str, frame_id, req_first_ts, actual_first_ts, (uint32_t)(actual_first_ts - req_first_ts));
        }

        // (5) Should hold : actual_last_ts + sample_time_us >= req_last_ts
        //  Make sure that there are not missing samples at the end of the frame.
        if (actual_last_ts + sample_time_us < req_last_ts)
        {
            IS_ERR(IS_ERR_SAMPLER_LAST_MISSING_SAMPLE,
                "%s: frame_id %u: Last requested timestamp (%" PRIu64 ") is larger than received timestamp (%" PRIu64 "). Delta [usec] = %u",
                _sampler_str, frame_id, req_last_ts, actual_last_ts, (uint32_t)(req_last_ts - actual_last_ts));
        }

        return true;
    }

    bool is_vaild_samples(const samples_data_t* samples, uint32_t frame_id) const
    {
        (void)frame_id;

        for (uint32_t k = 0; k < samples->num_elements; k++)
        {
            const uint64_t t = samples->samples[k].ts;
            if (0 == t) // shouldn't be happening, data is corrupted, will cause some crash later as well. probably all samples with time zero.
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "%s: frame_id %u: Time stamps equal zero ! corrupted gyro data, debug is needed.", _sampler_str, frame_id);
                return false;
            }
        }

        // Delta in micro seconds between consecutive gyro samples
        const float samples_delta_est_us = (1000000.0f / _estimated_frequency);

        for (uint32_t k = 1; k < samples->num_elements; k++)
        {
            const uint64_t t1 = samples->samples[k - 1].ts;
            const uint64_t t2 = samples->samples[k].ts;
            const sample_data_t* const s1 = &samples->samples[k - 1];
            const sample_data_t* const s2 = &samples->samples[k];

            if (t1 > t2)
            {
                IS_ERR(IS_ERR_SAMPLE_OOO,
                    "%s: frame_id %u: Time stamps are not strictly monotonically increasing: %" PRIu64 ", %" PRIu64, _sampler_str, frame_id, t1, t2);
                // Non strictly monotonically increasing samples will be ignored.
                // In case of duplicated timestamps first sample will be taken
            }
            else if (t1 == t2)
            {
                IS_ERR(IS_ERR_SAMPLE_DUPLICATED, "%s: frame_id %u: Time stamps are equal, debug is needed : %" PRIu64 "", _sampler_str, frame_id, t1);

                if (0 != memcmp(s1, s2, sizeof(*s1)))
                {
                    IS_ERR(IS_ERR_SAMPLE_DUPLICATED,
                        "%s: frame_id %u: Time stamps are equal but data samples are not: (%" PRIu64 "; %lf, %lf, %lf), (%" PRIu64 "; %lf, %lf, %lf)",
                        _sampler_str, frame_id,
                        s1->ts, s1->data[0], s1->data[1], s1->data[2],
                        s2->ts, s2->data[0], s2->data[1], s2->data[2]);
                }
            }
            else
            {
                // delta is smaller then ~35 minutes
                IS_ASSERT(t2 <= (0x7FFFFFFF + t1));

                if ((ABS((double)(t2 - t1) - samples_delta_est_us) / samples_delta_est_us) > 1.2)     /* Miss more then one gyro sample  */
                {
                    const uint32_t sampling_gap = (uint32_t)(t2 - t1);
                    const int32_t num_missing_samples = (int32_t)round((double)(t2 - t1) / samples_delta_est_us) - 1;
                    IS_ERR(IS_ERR_SAMPLE_SPACING, "%s: frame_id %u: Invalid samples spacing: %" PRIu64 ", %" PRIu64 ", %.2fHz ; Loss of %u[us] of data, estimated %d missing samples",
                        _sampler_str, frame_id, t1, t2, _estimated_frequency, sampling_gap, num_missing_samples);
                }
            }
        }

        return true;
    }


protected:    // members
    static const uint32_t       _max_sampler_srt_length = 32;

    const uint32_t              _max_frequency;                         /**< Maximal frequency [Hz] */
    const bool                  _relaxed_last_requested_interval_check;

    char                        _sampler_str[_max_sampler_srt_length];
    RingBuffer<float>           _estimated_frequencies;                 /**< Ring of estimated frequencies [Hz] */

public:     // members
    const float                 _noise_floor;                           /**< Noise floor */

    float                       _estimated_frequency;                   /**< Average estimated frequency [Hz] */
    const float                 _requested_frequency;                   /**< Requested frequency [Hz] */
    samples_times_t             _last_requested_interval;               /**< Last requested time interval */
    sample_data_t               _last_sample;                           /**< Last sample */
    rotate_sample*              _rotate;                                /**< Samples rotation function */

public:     // functions
    SensorDataHandler(
        const SensorInitInfo&       init,
        const char*                 sampler_str,
        bool                        relaxed_last_requested_interval_check,
        int32_t&                    status)
    :
        _max_frequency(MAX(MAX_REQ_FREQUENCY, (uint32_t)init._requested_frequency)),
        _relaxed_last_requested_interval_check(relaxed_last_requested_interval_check),
        _estimated_frequencies(GYRO_EST_MOVING_AVG_SUPPORT),
        _noise_floor(init._noise_floor),
        _estimated_frequency(init._requested_frequency),
        _requested_frequency(init._requested_frequency),
        _rotate(do_nothing)
    {
        IS_ASSERT(IS_RET_SUCCESS == status);

        _last_sample.ts = 0;
        _last_sample.data[0] = 0;
        _last_sample.data[1] = 0;
        _last_sample.data[2] = 0;
        _last_sample.data[3] = 0;

        _last_requested_interval.first_ts = 0;
        _last_requested_interval.last_ts = 0;

        strlcpy(_sampler_str, sampler_str, _max_sampler_srt_length);

        update_rotate(init._sensor_mount_angle, init._camera_position , status);
        if (status != IS_RET_SUCCESS)
        {
            IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: constructor failed. update_rotate()", sampler_str);
            goto Exit;
        }

    Exit:
        if (IS_RET_SUCCESS != status)
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: Constructor failed", sampler_str);
        }
    }

    virtual ~SensorDataHandler() {}

    void set_first_frame()
    {
        _last_sample.ts = 0;
        for (uint32_t idx = 0; idx < ARR_SIZE(_last_sample.data); idx++)
        {
            _last_sample.data[idx] = 0;
        }
    }

    bool is_valid(const samples_data_t* samples, uint32_t frame_id, bool is_first_frame, bool is_sensor_switch) const
    {
        if ((samples->num_elements > 0) && (NULL == samples->samples))
        {
            IS_ERR(IS_ERR_INVALID_INPUT, "%s: Input mismatch, num_elements > 0 but samples array is NULL", _sampler_str);
            return false;
        }

        if (0 == samples->num_elements) // will go in termination mode for last frames
        {
            return true;
        }

        if (true == is_first_frame)
        {
            const float requested_frequency = _requested_frequency;
            const float estimated_frequency = _estimated_frequency;
            const float gyro_freq_hz_error_ratio = ABS(requested_frequency - estimated_frequency) / requested_frequency;

            if (SAMPLES_EST_RATIO_ERROR_MARGIN_VS_AVG < gyro_freq_hz_error_ratio) // gyro rate is different between frames
            {
                IS_ERR(IS_ERR_SAMPLE_RATE_EST_VS_INIT,
                    "%s: estimated_frequency = %.1fHz is different from requested by input = %.1fHz",
                    _sampler_str, estimated_frequency, requested_frequency);
            }
        }

        is_valid_frequency(samples, frame_id);

        uint32_t err_cnt = 0;
        if (true != is_valid_frame_time(samples, frame_id))
        {
            err_cnt++;
        }

        if (true != is_vaild_samples(samples, frame_id))
        {
            err_cnt++;
        }

        if (false == is_sensor_switch)
        {
            if (true != is_valid_samples_spacings(samples, frame_id))
            {
                err_cnt++;
            }
        }

        if (true != is_valid_new_samples(samples, frame_id))
        {
            err_cnt++;
        }

        return (0 == err_cnt);
    }

    // checks if there is at least one new sample
    bool is_valid_new_samples(const samples_data_t* samples, uint32_t frame_id) const
    {
        (void)frame_id;

        IS_ASSERT(samples->num_elements > 0); // Otherwise should have not gotten here ...

        for (uint32_t idx = 0; idx < samples->num_elements; idx++)
        {
            if (samples->samples[idx].ts > _last_sample.ts)
            {
                return true; // at least one new sample was found ...
            }
        }

        IS_ERR(IS_ERR_SAMPLER_MISSING_MANY_SAMPLES,
            "%s: frame_id %u: Invalid input - Received only old samples: "
            "last sample timestamp %" PRIu64 " ; "
            "last sample timestamp from input %" PRIu64 " ; "
            "input number of elements %u. ",
            _sampler_str, frame_id,
            _last_sample.ts,
            samples->samples[samples->num_elements - 1].ts,
            samples->num_elements);

        return false;
    }

    void update_estimated_frequency(const samples_data_t* samples)
    {
        // Update estimated gyro sample rate
        float estimated_frequency = estimate_frequency(samples);
        if (0 == estimated_frequency)
        {
            estimated_frequency = _requested_frequency;
        }

        _estimated_frequencies.push(estimated_frequency);

        float sum = 0;

        for (uint32_t idx = 0; idx < _estimated_frequencies.size(); idx++)
        {
            sum += _estimated_frequencies[idx];
        }

        _estimated_frequency = sum / _estimated_frequencies.size();
        IS_ASSERT(_estimated_frequency > 0);
    }

    void update_rotate(uint32_t sensor_mount_angle, CameraPosition camera_position, int32_t& status)
    {
        if (camera_position == CAM_FRONT)
        {
            switch (sensor_mount_angle)
            {
            case 90:
            {
                _rotate = front_90;
                break;
            }

            case 270:
            {
                _rotate = front_270;
                break;
            }

            case 0:
            {
                _rotate = front_0;
                break;
            }

            case 180:
            {
                _rotate = front_180;
                break;
            }

            default:
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "Invalid sensor_mount_angle: %u", sensor_mount_angle);
                status = IS_RET_INVALID_INPUT;
            }
            }
        }
        else if (camera_position == CAM_REAR)
        {
            switch (sensor_mount_angle)
            {
            case 90:
            {
                _rotate = rear_90;
                break;
            }

            case 270:
            {
                _rotate = rear_270;
                break;
            }

            case 0:
            {
                _rotate = rear_0;
                break;
            }

            case 180:
            {
                _rotate = rear_180;
                break;
            }

            default:
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "Invalid sensor_mount_angle: %u", sensor_mount_angle);
                status = IS_RET_INVALID_INPUT;
            }
            }
        }
        else
        {
            IS_ERR(IS_ERR_INVALID_INPUT, "Invalid camera_position: %u", (uint32_t)camera_position);
            status = IS_RET_INVALID_INPUT;
        }
    }

    // Code was generated using generate_rotate_func.m
    void update_rotate_axes(int32_t axes_1, int32_t axes_2, int32_t axes_3, int32_t& status)
    {
        status = 0;

        if ((axes_1 == 1) && (axes_2 == 2) && (axes_3 == 3)) { _rotate = rotate_p1_p2_p3; }
        else if ((axes_1 == 1) && (axes_2 == 2) && (axes_3 == -3)) { _rotate = rotate_p1_p2_n3; }
        else if ((axes_1 == 1) && (axes_2 == -2) && (axes_3 == 3)) { _rotate = rotate_p1_n2_p3; }
        else if ((axes_1 == 1) && (axes_2 == -2) && (axes_3 == -3)) { _rotate = rotate_p1_n2_n3; }
        else if ((axes_1 == -1) && (axes_2 == 2) && (axes_3 == 3)) { _rotate = rotate_n1_p2_p3; }
        else if ((axes_1 == -1) && (axes_2 == 2) && (axes_3 == -3)) { _rotate = rotate_n1_p2_n3; }
        else if ((axes_1 == -1) && (axes_2 == -2) && (axes_3 == 3)) { _rotate = rotate_n1_n2_p3; }
        else if ((axes_1 == -1) && (axes_2 == -2) && (axes_3 == -3)) { _rotate = rotate_n1_n2_n3; }
        else if ((axes_1 == 1) && (axes_2 == 3) && (axes_3 == 2)) { _rotate = rotate_p1_p3_p2; }
        else if ((axes_1 == 1) && (axes_2 == 3) && (axes_3 == -2)) { _rotate = rotate_p1_p3_n2; }
        else if ((axes_1 == 1) && (axes_2 == -3) && (axes_3 == 2)) { _rotate = rotate_p1_n3_p2; }
        else if ((axes_1 == 1) && (axes_2 == -3) && (axes_3 == -2)) { _rotate = rotate_p1_n3_n2; }
        else if ((axes_1 == -1) && (axes_2 == 3) && (axes_3 == 2)) { _rotate = rotate_n1_p3_p2; }
        else if ((axes_1 == -1) && (axes_2 == 3) && (axes_3 == -2)) { _rotate = rotate_n1_p3_n2; }
        else if ((axes_1 == -1) && (axes_2 == -3) && (axes_3 == 2)) { _rotate = rotate_n1_n3_p2; }
        else if ((axes_1 == -1) && (axes_2 == -3) && (axes_3 == -2)) { _rotate = rotate_n1_n3_n2; }
        else if ((axes_1 == 2) && (axes_2 == 1) && (axes_3 == 3)) { _rotate = rotate_p2_p1_p3; }
        else if ((axes_1 == 2) && (axes_2 == 1) && (axes_3 == -3)) { _rotate = rotate_p2_p1_n3; }
        else if ((axes_1 == 2) && (axes_2 == -1) && (axes_3 == 3)) { _rotate = rotate_p2_n1_p3; }
        else if ((axes_1 == 2) && (axes_2 == -1) && (axes_3 == -3)) { _rotate = rotate_p2_n1_n3; }
        else if ((axes_1 == -2) && (axes_2 == 1) && (axes_3 == 3)) { _rotate = rotate_n2_p1_p3; }
        else if ((axes_1 == -2) && (axes_2 == 1) && (axes_3 == -3)) { _rotate = rotate_n2_p1_n3; }
        else if ((axes_1 == -2) && (axes_2 == -1) && (axes_3 == 3)) { _rotate = rotate_n2_n1_p3; }
        else if ((axes_1 == -2) && (axes_2 == -1) && (axes_3 == -3)) { _rotate = rotate_n2_n1_n3; }
        else if ((axes_1 == 2) && (axes_2 == 3) && (axes_3 == 1)) { _rotate = rotate_p2_p3_p1; }
        else if ((axes_1 == 2) && (axes_2 == 3) && (axes_3 == -1)) { _rotate = rotate_p2_p3_n1; }
        else if ((axes_1 == 2) && (axes_2 == -3) && (axes_3 == 1)) { _rotate = rotate_p2_n3_p1; }
        else if ((axes_1 == 2) && (axes_2 == -3) && (axes_3 == -1)) { _rotate = rotate_p2_n3_n1; }
        else if ((axes_1 == -2) && (axes_2 == 3) && (axes_3 == 1)) { _rotate = rotate_n2_p3_p1; }
        else if ((axes_1 == -2) && (axes_2 == 3) && (axes_3 == -1)) { _rotate = rotate_n2_p3_n1; }
        else if ((axes_1 == -2) && (axes_2 == -3) && (axes_3 == 1)) { _rotate = rotate_n2_n3_p1; }
        else if ((axes_1 == -2) && (axes_2 == -3) && (axes_3 == -1)) { _rotate = rotate_n2_n3_n1; }
        else if ((axes_1 == 3) && (axes_2 == 1) && (axes_3 == 2)) { _rotate = rotate_p3_p1_p2; }
        else if ((axes_1 == 3) && (axes_2 == 1) && (axes_3 == -2)) { _rotate = rotate_p3_p1_n2; }
        else if ((axes_1 == 3) && (axes_2 == -1) && (axes_3 == 2)) { _rotate = rotate_p3_n1_p2; }
        else if ((axes_1 == 3) && (axes_2 == -1) && (axes_3 == -2)) { _rotate = rotate_p3_n1_n2; }
        else if ((axes_1 == -3) && (axes_2 == 1) && (axes_3 == 2)) { _rotate = rotate_n3_p1_p2; }
        else if ((axes_1 == -3) && (axes_2 == 1) && (axes_3 == -2)) { _rotate = rotate_n3_p1_n2; }
        else if ((axes_1 == -3) && (axes_2 == -1) && (axes_3 == 2)) { _rotate = rotate_n3_n1_p2; }
        else if ((axes_1 == -3) && (axes_2 == -1) && (axes_3 == -2)) { _rotate = rotate_n3_n1_n2; }
        else if ((axes_1 == 3) && (axes_2 == 2) && (axes_3 == 1)) { _rotate = rotate_p3_p2_p1; }
        else if ((axes_1 == 3) && (axes_2 == 2) && (axes_3 == -1)) { _rotate = rotate_p3_p2_n1; }
        else if ((axes_1 == 3) && (axes_2 == -2) && (axes_3 == 1)) { _rotate = rotate_p3_n2_p1; }
        else if ((axes_1 == 3) && (axes_2 == -2) && (axes_3 == -1)) { _rotate = rotate_p3_n2_n1; }
        else if ((axes_1 == -3) && (axes_2 == 2) && (axes_3 == 1)) { _rotate = rotate_n3_p2_p1; }
        else if ((axes_1 == -3) && (axes_2 == 2) && (axes_3 == -1)) { _rotate = rotate_n3_p2_n1; }
        else if ((axes_1 == -3) && (axes_2 == -2) && (axes_3 == 1)) { _rotate = rotate_n3_n2_p1; }
        else if ((axes_1 == -3) && (axes_2 == -2) && (axes_3 == -1)) { _rotate = rotate_n3_n2_n1; }
        else {
            status = IS_RET_INVALID_INPUT;
            IS_ERR(IS_ERR_INVALID_INPUT, "Invalid ois_orientation_axis: %d, %d, %d", axes_1, axes_2, axes_3);
        }
    }
};

// This is not for gyro which have single instance for all camera sensors
class MultiSensorDataHandler
{
protected:
    //common info to all sensors
    const uint32_t              _num_active_sensors;
    const uint32_t              _num_sensors;

    uint32_t                    _sensor_map[EIS_MAX_NUM_SENSORS];        /**<  maps CamX sensor_idx into internal sensor_idx (since there is no reason to keep sensors without OIS) */
    uint32_t                    _sensor_map_inv[EIS_MAX_NUM_SENSORS];    /**<  maps internal sensor_idx into CamX sensor_idx (since there is no reason to keep sensors without OIS) */
    SensorDataHandler*          _sensors;

public:
    MultiSensorDataHandler(const MultiSensorInitInfo& init, const char* sampler_str, int32_t& status)
    :
        _num_active_sensors(init._num_active_sensors),
        _num_sensors(init._num_sensors),
        _sensors(NULL)
    {
        // Sanity
        IS_ASSERT(IS_RET_SUCCESS == status);
        IS_ASSERT(_num_active_sensors <= _num_sensors);
        IS_ASSERT(_num_active_sensors > 0);

        // Init sensor map
        {
            //TODO (etsairi): change below lines to:
            //                  _sensor_map = (uint32_t *)malloc(sizeof(*_sensor_map) * _num_active_sensors);
            //                  _sensor_map_inv = (uint32_t *)malloc(sizeof(*_sensor_map) * _num_sensors);
            // Right now VS code analysis is complaining ...

            if ( _num_sensors > EIS_MAX_NUM_SENSORS)
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "_num_sensors %u exceeds EIS_MAX_NUM_SENSORS %d", _num_sensors, EIS_MAX_NUM_SENSORS);
                status = IS_RET_INVALID_INPUT;
                goto Exit;
            }

            memset(_sensor_map,     0, sizeof(_sensor_map));
            memset(_sensor_map_inv, 0, sizeof(_sensor_map_inv));

            uint32_t map_idx = 0;
            for (uint32_t idx = 0; idx < _num_sensors; idx++)
            {
                const uint32_t freq = init.get_frequency(idx);
                if (0 == freq)
                {
                    _sensor_map_inv[idx] = 0xFFFFFFFF;  // Invalid sensor number - very big number
                    continue;
                }

                _sensor_map[map_idx] = idx;
                _sensor_map_inv[idx] = map_idx;
                map_idx++;
            }

            IS_ASSERT(map_idx > 0);
            IS_ASSERT(map_idx == _num_active_sensors);
        }

        // Init per sensor data
        {
            _sensors = (SensorDataHandler *)malloc(sizeof(*_sensors) * _num_active_sensors);
            if (NULL == _sensors)
            {
                IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "SensorDataHandler");
                status = IS_RET_OUT_OF_MEMORY;
                goto Exit;
            }

            for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
            {
                const uint32_t sensor_idx = _sensor_map[idx];

                const uint32_t freq = init.get_frequency(sensor_idx);
                IS_ASSERT(freq > 0);

                const float noise_floor = init.get_noise_floor(sensor_idx);
                IS_ASSERT(noise_floor >= 0);

                SensorInitInfo tmp((float)freq,
                                    noise_floor,
                                    init._num_axis,
                                    init._init_sensor[sensor_idx].sensor_mount_angle,
                                    init._init_sensor[sensor_idx].camera_position);

                char tmp_str[32] = { 0 };
                (void)snprintf(tmp_str, ARR_SIZE(tmp_str) - 1, "%s_#%u", sampler_str, _sensor_map[idx]);

                new(&_sensors[idx]) SensorDataHandler(tmp, tmp_str, true, status);
                if (IS_RET_SUCCESS != status)
                {
                    IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Constructor failed - %d", tmp_str, status);
                    goto Exit;
                }
            }
        }

    Exit:
        if (status != IS_RET_SUCCESS)
        {
            IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Constructor failed - %d", sampler_str, status);
        }
    };

    ~MultiSensorDataHandler()
    {
        if (NULL != _sensors)
        {
            for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
            {
                _sensors[idx].~SensorDataHandler();
            }

//            delete[](std::nothrow, _sensors);
//            _sensors = NULL;
            FREE(_sensors);
        }
    };

    void set_first_frame()
    {
        for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
        {
            _sensors[idx].set_first_frame();
        }
    }

    void update_estimated_frequency(const samples_data_t* samples, uint32_t sensor_id)
    {
        IS_ASSERT(true == has_sensor(sensor_id));

        const uint32_t mapped_sensor_idx = _sensor_map_inv[sensor_id];
        _sensors[mapped_sensor_idx].update_estimated_frequency(samples);
    }

    bool has_sensor(uint32_t sensor_idx) const
    {
        const uint32_t mapped_sensor_idx = _sensor_map_inv[sensor_idx];
        return (mapped_sensor_idx < _num_active_sensors);
    }

    samples_times_t& last_requested_interval(uint32_t sensor_id) const
    {
        IS_ASSERT(true == has_sensor(sensor_id));
        const uint32_t mapped_sensor_idx = _sensor_map_inv[sensor_id];

        return _sensors[mapped_sensor_idx]._last_requested_interval;
    }

    sample_data_t& last_sample(uint32_t sensor_id)
    {
        IS_ASSERT(true == has_sensor(sensor_id));
        const uint32_t mapped_sensor_idx = _sensor_map_inv[sensor_id];

        return _sensors[mapped_sensor_idx]._last_sample;
    }

    float estimated_frequency(uint32_t sensor_id) const
    {
        IS_ASSERT(true == has_sensor(sensor_id));
        const uint32_t mapped_sensor_idx = _sensor_map_inv[sensor_id];

        return _sensors[mapped_sensor_idx]._estimated_frequency;
    }

    float requested_frequency(uint32_t sensor_id) const
    {
        IS_ASSERT(true == has_sensor(sensor_id));
        const uint32_t mapped_sensor_idx = _sensor_map_inv[sensor_id];

        return _sensors[mapped_sensor_idx]._requested_frequency;
    }

protected:
    bool is_valid(
        const samples_data_t*   samples,
        uint32_t                active_sensor_idx,
        uint32_t                frame_id,
        bool                    is_first_frame,
        const char*             sampler_str,
        bool                    is_sensor_switch) const
    {
        (void)sampler_str;

        const uint32_t mapped_sensor_idx = _sensor_map_inv[active_sensor_idx];

        uint32_t num_errors = 0;

        if (false == has_sensor(active_sensor_idx))
        {
            if ((NULL != samples) && (samples->num_elements > 0))
            {
                IS_ERR(IS_ERR_INVALID_INPUT,
                    "%s: frame_id %u : Sensor #%u is disabled but %u samples were received. Samples will be ignored",
                    sampler_str, frame_id, active_sensor_idx, samples->num_elements);
            }
        }
        else
        {
            if (true != _sensors[mapped_sensor_idx].is_valid(samples, frame_id, is_first_frame, is_sensor_switch))
            {
                num_errors++;
            }
        }

        if (num_errors > 0)
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: frame_id %u : Invalid data from sensor #%u",
                sampler_str, frame_id, active_sensor_idx);
            return false;
        }

        return true;
    }
};

/*********************************************************************************************************************/
/*                  Data handlers classes                                                                            */
/*********************************************************************************************************************/

class OISHandler : public MultiSensorDataHandler, public MultiSensorDataBuffer<sample_data_t>
{
private: // definitions

    class OISFilter
    {
        class LongFilter
        {
            typedef struct _frame_data_s
            {
                double      mean;
                double      max;
                double      min;
                uint32_t    num_samples;
            } frame_data_t;

            static const uint32_t                 _max_num_samples_to_mean = 500; /**< max number of samples to determine whether the signal is stable */
            static const uint32_t                 _min_num_samples_to_mean = 300; /**< min number of samples to determine whether the signal is stable */

            static const uint32_t                 _min_num_samples_to_long = 25;  /**< Below the threshold the filter will not enter steady state */

            const float                           _max_iir_alpha = 0.95f;         /**< Sets the filter strength in steady state */
            const float                           _iir_alpha_inc_delta = 0.2f;    /**< How quickly the filter goes into steady state, per sample */
            const float                           _iir_alpha_dec_delta = 0.2f;    /**< How quickly the filter goes out of steady state, per sample */

            //TODO (etsairi): change to 0.25-0.5 deg !
            const float                           _steady_state_th = 1000.0f;     /**< Above the threshold the filter will not enter steady state */

            RingBuffer<frame_data_t>              _frame_data;                    /**< Only the frames that will determine whether to enter a stable state */
            uint32_t                              _total_samples_counter;         /**< Current number of samples to determine whether the signal is stable */
            double                                _total_samples_mean;

            double                                _latest_filtered_sample;
            bool                                  _is_first_frame;

            float                                 _iir_alpha;

            double*                               _prev_out_samples;              /**< Previous LongFilter output - samples */
            uint64_t*                             _prev_out_timestamps;           /**< Previous LongFilter output - timestamps */
            uint32_t                              _prev_out_size;                 /**< Previous LongFilter output - size*/

            enum FilterState { Extend, Shorten }  _state;                         /**< Determines whether to increase or decrease past frames data */

            const float                           _noise_floor;                   /**< Noise floor */

        public:
            LongFilter(float estimated_frequency, float noise_floor, uint32_t max_out_size, int32_t& status)
            :
                _frame_data(calc_max_mean_frames(estimated_frequency)),
                _total_samples_counter(0),
                _total_samples_mean(0),
                _latest_filtered_sample(0),
                _is_first_frame(true),
                _iir_alpha(0),
                _prev_out_samples(NULL),
                _prev_out_timestamps(NULL),
                _prev_out_size(0),
                _state(Extend),
                _noise_floor(noise_floor)
            {
                if (0 == _frame_data.maxSize())
                {
                    IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "LongFilter");
                    status = IS_RET_OUT_OF_MEMORY;
                    goto Exit;
                }

                _prev_out_samples = (double*)malloc(sizeof(*_prev_out_samples) * max_out_size);
                if (NULL == _prev_out_samples)
                {
                    IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "LongFilter");
                    status = IS_RET_OUT_OF_MEMORY;
                    goto Exit;
                }

                _prev_out_timestamps = (uint64_t*)malloc(sizeof(*_prev_out_timestamps) * max_out_size);
                if (NULL == _prev_out_timestamps)
                {
                    IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "LongFilter");
                    status = IS_RET_OUT_OF_MEMORY;
                    goto Exit;
                }

            Exit:
                if (IS_RET_SUCCESS != status)
                {
                    IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Constructor failed - %d", "LongFilter", status);
                }

            }

            ~LongFilter()
            {
                FREE(_prev_out_samples);
                FREE(_prev_out_timestamps);
            }

            void set_first_frame()
            {
                _frame_data.clear();
                _total_samples_counter  = 0;
                _total_samples_mean     = 0;
                _latest_filtered_sample = 0;
                _is_first_frame         = true;
                _iir_alpha              = 0;
                _prev_out_size          = 0;
                _state                  = Extend;
            }

            uint32_t prev_sync_and_find_new_sample_start_idx(double* samples, const uint64_t* timestamps, uint32_t size)
            {
                uint32_t new_sample_idx = 0;
                for (uint32_t i = 0; new_sample_idx < size && i < _prev_out_size; i++)
                {
                    if (timestamps[new_sample_idx] > _prev_out_timestamps[i])
                    {
                        continue;
                    }
                    else if (timestamps[new_sample_idx] == _prev_out_timestamps[i])
                    {
                        samples[new_sample_idx] = _prev_out_samples[i];
                        new_sample_idx++;
                    }
                    else
                    {
                        IS_ASSERT(0); // Timestamps must be monotonous
                    }
                }
                IS_ASSERT(new_sample_idx < size - 1); // There must be at least one new sample
                return new_sample_idx;
            }

            void filter(double* samples, const uint64_t* timestamps, uint32_t size)
            {
                const uint32_t new_sample_idx = prev_sync_and_find_new_sample_start_idx(samples, timestamps, size);
                const frame_data_t frame_sample = create_frame_data(samples, size, new_sample_idx);
                add_frame_data(frame_sample);
                adjust_size_and_limitations_after_add_frame_data();

                const bool past_close_to_mean = is_past_samples_close_to_new_mean();
                bool curr_close_to_mean = false;
                bool curr_close_to_steady_state_th = false;

                if (_is_first_frame)
                {
                    _latest_filtered_sample = samples[new_sample_idx];
                    _is_first_frame = false;
                }

                for (uint32_t i = new_sample_idx; i < size; i++)
                {
                    curr_close_to_mean = ABS(samples[i] - _total_samples_mean) < _noise_floor;
                    curr_close_to_steady_state_th = ABS(samples[i]) < _steady_state_th;
                    if (past_close_to_mean && curr_close_to_mean && curr_close_to_steady_state_th)
                    {
                        _iir_alpha = MIN(_iir_alpha + _iir_alpha_inc_delta, _max_iir_alpha);
                    }
                    else
                    {
                        _iir_alpha = MAX(_iir_alpha - _iir_alpha_dec_delta, 0);
                    }

                    samples[i] = samples[i] * (1 - _iir_alpha) + _latest_filtered_sample * _iir_alpha;
                    _latest_filtered_sample = samples[i];
                }

                if (past_close_to_mean && curr_close_to_mean && curr_close_to_steady_state_th)
                {
                    _state = Extend;
                }
                else
                {
                    _state = Shorten;
                }

                // TODO (asimchi): consider keeping previous buffer in OIS Filter class (next to current) and swap pointers.
                memcpy(_prev_out_samples, samples, sizeof(*_prev_out_samples)*size);
                memcpy(_prev_out_timestamps, timestamps, sizeof(*_prev_out_timestamps)*size);
                _prev_out_size = size;
            }

        private:
            static inline uint32_t calc_max_mean_frames(float estimated_frequency)
            {
                /**
                *   - The lower bound is if we get only one ois sample per frame but it is not likely at all.
                *     In this case buffer size should be as max past-samples-window size (_max_num_samples_to_mean).
                *
                *   - Optimized to return _max_num_samples_to_mean/X, that assume we get average of X sample per frame.
                *     X depends on estimated_frequency and max FPS.
                **/

                const float lower_bound_of_num_ois_samples_per_frame = (estimated_frequency / EIS_MAX_FPS) * MIN_FREQ_SAFETY_MARGIN;
                return (uint32_t)ceil(_max_num_samples_to_mean / lower_bound_of_num_ois_samples_per_frame);
            }

            inline bool is_possible_to_remove_frame() const
            {
                if (_total_samples_counter > _frame_data(0).num_samples)
                {
                    return _total_samples_counter - _frame_data(0).num_samples > _min_num_samples_to_mean;
                }
                else
                {
                    return false;
                }
            }

            frame_data_t create_frame_data(const double* samples, uint32_t size, uint32_t new_sample_idx)
            {
                IS_ASSERT(new_sample_idx < size - 1); // There must be at least one new sample

                frame_data_t frame_sample;
                frame_sample.num_samples = 1;
                frame_sample.max = samples[new_sample_idx];
                frame_sample.min = samples[new_sample_idx];
                frame_sample.mean = samples[new_sample_idx];

                for (uint32_t i = new_sample_idx + 1; i < size; i++)
                {
                    if (samples[i] > frame_sample.max)
                    {
                        frame_sample.max = samples[i];
                    }
                    else if (samples[i] < frame_sample.min)
                    {
                        frame_sample.min = samples[i];
                    }
                    frame_sample.mean += samples[i];
                    frame_sample.num_samples++;
                }
                frame_sample.mean /= frame_sample.num_samples;

                return frame_sample;
            }

            void add_frame_data(const frame_data_t& frame_sample)
            {
                // Calculation of the maximum mean frames should prevent the buffer from filling up.
                IS_ASSERT(_frame_data.size() + 1 <= _frame_data.maxSize());

                const double curr_sum = _total_samples_mean * _total_samples_counter;
                const double new_sum = frame_sample.mean * frame_sample.num_samples;

                _total_samples_counter += frame_sample.num_samples;
                _total_samples_mean = (curr_sum + new_sum) / _total_samples_counter;

                _frame_data.push(frame_sample);
            }

            void remove_frame_data()
            {
                frame_data_t& oldest = _frame_data(0);

                const double curr_sum = _total_samples_mean * _total_samples_counter;
                const double oldest_sum = oldest.mean * oldest.num_samples;

                _total_samples_counter -= oldest.num_samples;
                _total_samples_mean = (curr_sum - oldest_sum) / _total_samples_counter;

                _frame_data.pop();
            }

            void adjust_size_and_limitations_after_add_frame_data()
            {
                if (_state == Shorten)
                {
                    uint32_t num_frame_to_remove = 2;
                    while (is_possible_to_remove_frame() && num_frame_to_remove > 0)
                    {
                        remove_frame_data();
                        num_frame_to_remove--;
                    }
                }

                while (_total_samples_counter > _max_num_samples_to_mean)
                {
                    remove_frame_data();
                }
            }

            bool is_past_samples_close_to_new_mean() const
            {
                if (_total_samples_counter < _min_num_samples_to_long)
                {
                    return false;
                }

                if (_frame_data.size() <= 1)
                {
                    return true;
                }

                uint32_t past_size = _frame_data.size() - 1; // without current frame data
                for (uint32_t i = 0; i < past_size; i++)
                {
                    if ((ABS(_frame_data[i].max - _total_samples_mean) > _noise_floor) ||
                        (ABS(_frame_data[i].min - _total_samples_mean) > _noise_floor))
                    {
                        return false;
                    }
                }
                return true;
            }
        }; // LongFilter

    private:
        const uint32_t          _filter_mode;

        Blackman                _short_filter;      /**< First filter - filters all input samples */
        const uint32_t          _support_time;      /**< Support required for the short filter in [microsecond] */
        const uint32_t          _max_out_size;      /**< First filter - filters all input samples */

        LongFilter              _long_filter_x;     /**< Second filter - strong filter of x-axis steady-state signal */
        LongFilter              _long_filter_y;     /**< Second filter - strong filter of y-axis steady-state signal */

        double*                 _out_samples;       /**< OIS Filter output - samples */
        uint64_t*               _out_timestamps;    /**< OIS Filter output - timestamps */
        uint32_t                _out_size;          /**< OIS Filter output - size*/

        bool                    _is_more_than_30_fps;
        bool                    _is_need_to_update_short_filter_weights;
        bool                    _is_use_long_filter;

    public:
        OISFilter(
            uint32_t    filter_mode,
            uint32_t    short_filter_radius,
            float       estimated_frequency,
            float       noise_floor,
            SensorData::ois_module_type_t ois_module_type,
            int32_t&    status)
        :
            _filter_mode(filter_mode),
            _short_filter(short_filter_radius, status),
            _support_time(calc_support_usec(_short_filter._radius, estimated_frequency)),
            _max_out_size(calc_max_out_size(_short_filter._radius, estimated_frequency)),
            _long_filter_x(estimated_frequency, noise_floor, _max_out_size, status),
            _long_filter_y(estimated_frequency, noise_floor, _max_out_size, status),
            _out_samples(NULL),
            _out_timestamps(NULL),
            _out_size(0),
            _is_more_than_30_fps(false),
            _is_need_to_update_short_filter_weights(true),
            _is_use_long_filter((ois_module_type != SensorData::GIMBAL) && (ois_module_type != SensorData::GIMBAL_PRISM))
        {
            _out_samples = (double*)malloc(sizeof(*_out_samples) *_max_out_size);
            if (NULL == _out_samples)
            {
                IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "OISFilter");
                status = IS_RET_OUT_OF_MEMORY;
                goto Exit;
            }

            _out_timestamps = (uint64_t*)malloc(sizeof(*_out_timestamps) *_max_out_size);
            if (NULL == _out_timestamps)
            {
                IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "OISFilter");
                status = IS_RET_OUT_OF_MEMORY;
                goto Exit;
            }

        Exit:
            if (IS_RET_SUCCESS != status)
            {
                IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Constructor failed - %d", "OISFilter", status);
            }
        }

        ~OISFilter()
        {
            FREE(_out_samples);
            FREE(_out_timestamps);
        }

        void set_first_frame()
        {
            _long_filter_x.set_first_frame();
            _long_filter_y.set_first_frame();
            _is_need_to_update_short_filter_weights = true;
        }

        void update_is_more_than_30_fps(bool ans)
        {
            if (0 != _filter_mode)
            {
                if (ans != _is_more_than_30_fps)
                {
                    _is_need_to_update_short_filter_weights = true;
                }

                _is_more_than_30_fps = ans;

                if (true == _is_need_to_update_short_filter_weights)
                {
                    uint32_t new_radius;
                    if (true == _is_more_than_30_fps)
                    {
                        new_radius = 5U;
                    }
                    else
                    {
                        new_radius = _filter_mode;
                    }
                    _short_filter.update_blackman_weights(new_radius);
                    _is_need_to_update_short_filter_weights = false;
                }

            }
        }

        void get_interp(
            uint64_t                            ts_start,
            uint64_t                            ts_end,
            uint32_t                            frame_id,
            uint32_t                            active_sensor_id,
            uint32_t                            samples_axis,
            const MultiSensorDataBuffer&        samples,
            uint32_t                            num_points,
            Point2d*                            out,
            SamplerInterpolationStatistics&     stats,
            const char*                         sampler_str)
        {
            uint32_t start_idx, end_idx;
            find_start_and_end_samples_indexes(ts_start, ts_end, active_sensor_id, samples, start_idx, end_idx);
            _out_size = _short_filter.filter_by_idx(samples._theta,
                                                    samples_axis,
                                                    start_idx,
                                                    end_idx,
                                                    _max_out_size,
                                                    _out_samples,
                                                    _out_timestamps);


            // This hack is used because we call this function twice, once for regular and once for get_device_rotation_delta, in case of gimbal or gimbal prism
            if (true == _is_use_long_filter)
            {
                LongFilter& long_filter = (samples_axis == 0) ? _long_filter_x : _long_filter_y;
                long_filter.filter(_out_samples, _out_timestamps, _out_size);
            }

            interp(ts_start, ts_end, frame_id, active_sensor_id, samples_axis, num_points, out, stats, sampler_str);
        }

    private:
        inline static uint32_t calc_support_usec(uint32_t short_filter_radius, float estimated_frequency)
        {
            return (uint32_t)(OIS_SHORT_FILTER_SAMPLES_SAFETY_MARGIN * short_filter_radius * 1000000.0f / estimated_frequency);
        }

        inline static uint32_t calc_max_out_size(uint32_t short_filter_radius, float estimated_frequency)
        {
            return (uint32_t)ceil(((estimated_frequency / EIS_MIN_FPS) * MAX_FREQ_SAFETY_MARGIN)
                + ((2 * short_filter_radius + 1) * OIS_SHORT_FILTER_SAMPLES_SAFETY_MARGIN));
        }

        void find_start_and_end_samples_indexes(
            uint64_t                            ts_start,
            uint64_t                            ts_end,
            uint32_t                            active_sensor_id,
            const MultiSensorDataBuffer&        samples,
            uint32_t&                           start_idx,
            uint32_t&                           end_idx) const
        {
            // Add support to filter edges
            uint64_t adjusted_ts_start = ts_start;
            if (ts_start > _support_time)
            {
                adjusted_ts_start = ts_start - _support_time;
            }
            else
            {
                IS_ASSERT(0); //UBSAN
            }

            const uint64_t  adjusted_ts_end = ts_end + _support_time;

            start_idx = samples.find_idx_in_theta(adjusted_ts_start, active_sensor_id);

            // Need to add one more sample from the start
            if (samples._theta[start_idx].ts > adjusted_ts_start)
            {
                if ((start_idx > 0) &&      // not first sample in buffer
                    (false == samples.was_sensor_changed(start_idx)) && // sensor was not changes w.r.t. previous sample
                    (false == samples.ts_delta_too_big(samples._theta[start_idx - 1].ts, samples._theta[start_idx].ts))) // previous sample distance is not too big
                {
                    start_idx--;
                }
            }
            else if (true == samples.ts_delta_too_big(samples._theta[start_idx].ts, samples._theta[start_idx + 1].ts))
            {
                // Check if the start points that we got are not related to other intervals
                start_idx++;
            }

            end_idx = samples.find_end_idx_in_theta(adjusted_ts_end, active_sensor_id, start_idx);

            IS_ASSERT((end_idx == (samples._theta.size() - 1)) ||  // last sample
                (false == samples.ts_delta_too_big(samples._theta[end_idx - 1].ts, samples._theta[end_idx].ts))); // still relevant w.r.t start_idx interval
        }

        void interp(
            uint64_t                            ts_start, // ts of start of frame, not first sample but actually second sample we get at out
            uint64_t                            ts_end,   // ts of end of frame, not last sample but actually one before last sample we get at out
            uint32_t                            frame_id,
            uint32_t                            active_sensor_id,
            uint32_t                            samples_axis,
            uint32_t                            num_points,
            Point2d*                            out,
            SamplerInterpolationStatistics&     stats,
            const char*                         sampler_str) const
        {
            (void)frame_id;
            (void)sampler_str;
            (void)active_sensor_id;

            if (0 == _out_size) // no valid samples if buffer after filtering at all. Take zero instead and print error.
            {
                for (uint32_t point_idx = 0; point_idx < num_points; point_idx++)
                {
                    out[point_idx].at(samples_axis) = 0.0;
                }

                IS_ERR(IS_ERR_SENSOR_MISSING_MANY_SAMPLES,
                    "%s: frame_id %u Sensor %u: axis-%s: filter extrapolated %u points with zero value since no valid samples exist in buffer",
                    sampler_str, frame_id, active_sensor_id, ((0 == samples_axis) ? "x" : "y"), num_points);

                stats.num_processed     = num_points;
                stats.num_error         = num_points; // All points are missing
                stats.num_exrap_start   = num_points; // All points are missing
                stats.num_exrap_end     = 0;
                return;
            }

            int32_t  warm_start_idx   = 0;
            memset(&stats, 0, sizeof(stats));

            IS_ASSERT(0 != _out_size);

            for (uint32_t point_idx = 0; point_idx < num_points; point_idx++)
            {
                const uint64_t ts = timestamp_interpolation(ts_start, ts_end, num_points, point_idx);

                uint32_t ts_idx = _out_size;
                //for (uint32_t idx = warm_start_idx; idx < _out_size - 1; idx++)
                for (uint32_t idx = warm_start_idx; (_out_size > 0) && (idx < _out_size - 1); idx++)
                {
                    if (_out_timestamps[idx + 1] > ts)
                    {
                        ts_idx = idx;
                        warm_start_idx = (idx >= 1) ? idx - 1 : 0;
                        break;
                    }
                }

                IS_ASSERT(ts_idx >= 0);
                IS_ASSERT(ts_idx <= _out_size);

                // interpolate / extrapolate this point
                const uint64_t  start_ts    = _out_timestamps[MIN(ts_idx, _out_size - 1)];
                const double    start       = _out_samples[MIN(ts_idx, _out_size - 1)];
                const uint64_t  mid_point   = ts;

                // if (ts_idx >= _out_size) i.e, index is out of buffer it will clamped to buffer size -1 which means ZOH extrapolation
                const uint64_t  end_ts      = _out_timestamps[MIN(ts_idx + 1, _out_size - 1)];
                const double    end         = _out_samples[MIN(ts_idx + 1, _out_size - 1)];

                if (start_ts > ts)  // Extrapolate from start
                {
                    out[point_idx].at(samples_axis) = start;
                    stats.num_exrap_start++;
                }
                else if (ts > end_ts)  // Extrapolate from end
                {
                    out[point_idx].at(samples_axis) = end;
                    stats.num_exrap_end++;
                }
                else // Interpolate
                {
                    if ((start_ts != mid_point) && (start_ts < end_ts) && (mid_point > start_ts))
                    {
                        IS_ASSERT(start_ts < end_ts);

                        IS_ASSERT(start_ts <= mid_point);
                        IS_ASSERT(end_ts >= mid_point);

                        // Sanity check - avoid devision by zero
                        IS_ASSERT(((int32_t)(end_ts - start_ts)) > IS_EPS);

                        // Sanity check - spacing between gyro samples should be less then ~35 minutes
                        IS_ASSERT(end_ts <= (0x7FFFFFFF + start_ts));

                        const uint32_t ts_delta = (uint32_t)(end_ts - start_ts);
                        const uint32_t point_ts_data = (uint32_t)(mid_point - start_ts);

                        out[point_idx].at(samples_axis) = start + (point_ts_data * (end - start)) / ts_delta;
                    }
                    else
                    {
                        out[point_idx].at(samples_axis) = start;
                    }
                }
            }

            stats.num_processed = num_points;
            stats.num_error     = stats.num_exrap_end + stats.num_exrap_start;

            if (stats.num_error > 0)
            {
                IS_ERR(IS_ERR_SENSOR_MISSING_MANY_SAMPLES,
                    "%s: frame_id %u Sensor %u: axis-%s: filter extrapolated %u points from start and %u points from end",
                    sampler_str,
                    frame_id,
                    active_sensor_id,
                    ((0 == samples_axis) ? "x" : "y"),
                    (uint32_t)stats.num_exrap_start,
                    (uint32_t)stats.num_exrap_end);
            }
        }
    }; // OISFilter

private: // members

    OISFilter*                  _filters;               /**< OIS filters, per OIS sensor */

    const uint32_t              _buffer_delay;          /**< Buffer delay */

    static const bool           _real_time_filter = true; // TODO (asimchi): depends on EIS instance

#if defined(_EIS_INTERNAL_DEBUG_)
    FILE*                       _debug_filter;
#endif /* _EIS_INTERNAL_DEBUG_ */

private: // static functions
    static bool is_tripod_mode(
        double                      noise_floor,
        const RingBuffer<double>&   in,
        uint32_t                    start_idx,
        uint32_t                    end_idx)
    {
        double max_val = -INFINITY;
        double min_val = INFINITY;

        for (uint32_t idx = start_idx; idx < end_idx; idx++)
        {
            const double M = in.at(idx);

            max_val = MAX(M, max_val);
            min_val = MIN(M, min_val);

            if ((max_val - min_val) > noise_floor)
            {
                return false;
            }
        }

        IS_ASSERT((max_val - min_val) <= noise_floor); // minimal and maximal values are close, so we can enter tripod mode
        return true;
    }

private: // methods

#if defined(_EIS_INTERNAL_DEBUG_)
    int32_t debug_data_init()
    {
        errno_t err;

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

        {
            char output_csv[1024];
            (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%sOIS_filtered.csv", g_output_csv_base_folder, INTERPOLATED_PREFIX_FILE_NAME);

            err = IS_FOPEN(&(_debug_filter), output_csv, "w");
            if (err != 0)
            {
                return IS_RET_GENERAL_ERROR;
            }
        }

        return IS_RET_SUCCESS;
    }
#endif /* _EIS_INTERNAL_DEBUG_ */

    bool is_valid_data_for_filter() const
    {
        //TODO (etsairi): implement
        return true;
    }

public: // methods
    OISHandler(
        const OISInitInfo&           init,
        uint32_t                     ois_short_filter_radius,
        int32_t&                     status)
    :
        MultiSensorDataHandler(init, "OIS", status),
        MultiSensorDataBuffer(init._num_axis, init._samples_ring_buffer_size, init._num_active_sensors, "OIS", status),
        _filters(NULL),
        _buffer_delay(init._init_common->buffer_delay)
    {
        if (IS_RET_SUCCESS != status)
        {
            goto Exit;
        }

        // Sanity check
        {
            IS_ASSERT((NULL != _sensor_id) == (_num_active_sensors > 1));
        }

        for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
        {
            const int32_t lens_pos_orientation_axis_x = init._init_sensor[_sensor_map[idx]].is_chromatix_info.top.lens_pos_orientation_axis_x;
            const int32_t lens_pos_orientation_axis_y = init._init_sensor[_sensor_map[idx]].is_chromatix_info.top.lens_pos_orientation_axis_y;
            const int32_t lens_pos_orientation_axis_z = init._init_sensor[_sensor_map[idx]].is_chromatix_info.top.lens_pos_orientation_axis_z;

            // Use lens pos orientation only if it was set, if it was set to be [0,0,0] then use gyro rotation.
            // This supports old branches where we pre-rotated the OIS to be like gyro
            if ((lens_pos_orientation_axis_x != 0) || (lens_pos_orientation_axis_y != 0) || (lens_pos_orientation_axis_z != 0))
            {
                int32_t status_tmp;

                // Use default value of 3 since we only use 2 axes and in Chromatix there will be probably 0
                //const int32_t lens_pos_orientation_axis_z_tmp = lens_pos_orientation_axis_z;
                const int32_t lens_pos_orientation_axis_z_tmp = 3;

                _sensors[idx].update_rotate_axes(lens_pos_orientation_axis_x, lens_pos_orientation_axis_y, lens_pos_orientation_axis_z_tmp, status_tmp);
                if (IS_RET_SUCCESS != status_tmp)
                {
                    status = status_tmp;
                    goto Exit;
                }
            }
        }

        // Init short filter per sensor
        {
            _filters = (OISFilter *)malloc(sizeof(*_filters) * _num_active_sensors);
            if (NULL == _filters)
            {
                IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Out of memory", "OISFilter");
                status = IS_RET_OUT_OF_MEMORY;
                goto Exit;
            }

            for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
            {
                char tmp_str[32] = { 0 };
                (void)snprintf(tmp_str, ARR_SIZE(tmp_str) - 1, "%s_#%u", "OIS", _sensor_map[idx]);

                const float frequency = (float)init.get_frequency(_sensor_map[idx]);
                const float noise_floor = init.get_noise_floor(_sensor_map[idx]);

                new(&_filters[idx]) OISFilter((uint32_t)init._init_sensor[_sensor_map[idx]].is_chromatix_info.ois__filter_mode,
                                              ois_short_filter_radius,
                                              frequency,
                                              noise_floor,
                                              init.get_ois_type(_sensor_map[idx]),
                                              status);
                if (IS_RET_SUCCESS != status)
                {
                    IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s #%u: Constructor failed - %d", "OISFilter", idx, status);
                    goto Exit;
                }
            }
        }

#if defined(_EIS_INTERNAL_DEBUG_)
        // Init debug info
        {
            int32_t tmp_status = debug_data_init();
            if (IS_RET_SUCCESS != tmp_status)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "%s: failed to init debug files", "OIS");
                status = tmp_status;
                goto Exit;
            }
        }
#endif /* _EIS_INTERNAL_DEBUG_ */

    Exit:
        if (status != IS_RET_SUCCESS)
        {
            IS_ERR(IS_ERR_OUT_OF_MEMORY, "%s: Constructor failed - %d", "OIS", status);
        }
    };

    virtual ~OISHandler()
    {
        if (NULL != _filters)
        {
            for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
            {
                _filters[idx].~OISFilter();
            }
            FREE(_filters);
        }

#if defined(_EIS_INTERNAL_DEBUG_)
        IS_FCLOSE(_debug_filter);
#endif /* _EIS_INTERNAL_DEBUG_ */
    };

    // - removes samples from previous frames
    // - rotate sample (according to camera position and sensor mount angle)
    // - adjust OIS time-stamps to agree with gyro time-line
    // - convert values from hall units to [pixels]
    //
    // - this function must be called once, and only once, per eis_process()
    //
    // returns the number of elements that were processed , negative number for error
    int32_t process_input(
        const ois_data_t*   ois_data,
        uint32_t            active_sensor_idx,
        int32_t             ois_offset)
    {
        IS_ASSERT(NULL != ois_data);

        if (false == has_sensor(active_sensor_idx))
        {
            return 0;  // Nothing to do ... sampler is disabled
        }

        if (ois_data->num_elements == 0)
        {
            return 0; // Nothing to do ...
        }

        IS_ASSERT(NULL != ois_data->samples);

        uint32_t cnt = 0;

        // get internal buffer sensor index from active sensor index
        const uint32_t mapped_sensor_idx = _sensor_map_inv[active_sensor_idx];

        for (uint32_t idx = 0; idx < ois_data->num_elements; idx++)
        {
            // Find first relevant sample (removes samples from previous frames)
            if (_sensors[mapped_sensor_idx]._last_sample.ts >= ois_data->samples[idx].ts)
            {
                continue;   /* remove samples from the past */
            }

            // here we use only new ois data
            cnt++;

            sample_data_t ois_rotated = ois_data->samples[idx];
            const uint64_t ts_backup = ois_rotated.ts;

            IS_ASSERT(ois_rotated.ts > (uint64_t)MAX(0, ois_offset));  // Sanity check

            _sensors[mapped_sensor_idx]._rotate(ois_rotated);         // Rotate the OIS sample to align to image coordinates system

            ois_rotated.ts = (uint64_t)((int64_t)ois_rotated.ts - (int64_t)ois_offset);
            // todo: (ton) : check is this + or  minus. should be minus !
            // anyway, why should we do in this confusing way, why not keep regular timeline and add offset when sampling, easier to understand

                                            //TODO (etsairi): make this sample uniform? w.r.t est. freq?

            // For later processing, of LDC + RS, we need to keep the shift done by OIS.
            // For the angle filtration we will convert the OIS to approximated rotation
            // which will be stored as a part of the gyro angle
            push_data(ois_rotated, active_sensor_idx);

            IS_ASSERT((NULL != _sensor_id) == (_num_active_sensors > 1));

            _sensors[mapped_sensor_idx]._last_sample = ois_rotated;
            _sensors[mapped_sensor_idx]._last_sample.ts = ts_backup;  // actual OIS ts

#if defined(_EIS_INTERNAL_DEBUG_)
            {
                IS_ASSERT(NULL != _debug_process);
                fprintf(_debug_process, "%u, %" PRIu64 ", %5.15lf, %5.15lf, %5.15lf, %5.15lf\n",
                    idx,
                    _sensors[mapped_sensor_idx]._last_sample.ts,
                    _sensors[mapped_sensor_idx]._last_sample.data[0],
                    _sensors[mapped_sensor_idx]._last_sample.data[1],
                    _sensors[mapped_sensor_idx]._last_sample.data[2],
                    _sensors[mapped_sensor_idx]._last_sample.data[3]);
            }
#endif /* _EIS_INTERNAL_DEBUG_ */
        }

        // Received only old samples, this could indicate on an issue...
        IS_ASSERT((ois_data->num_elements > 0) && (cnt > 0));

        if (cnt == 0) // avoid devision by zero later
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "Failed to updated OIS samples, all samples are old");
            return -1;
        }

        return cnt;
    }

    bool is_valid(const samples_data_t* samples, uint32_t active_sensor_idx, uint32_t frame_id, bool is_first_frame, bool is_sensor_switch) const
    {
        uint32_t err_cnt = 0;

        if (true != MultiSensorDataHandler::is_valid(samples, active_sensor_idx, frame_id, is_first_frame, _sampler_str, is_sensor_switch))
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: frame_id %u : Invalid data from sensor #%u",
                _sampler_str, frame_id, active_sensor_idx);
            err_cnt++;
        }

        if (true != is_valid_data_for_filter())
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: frame_id %u : Invalid data for filter", _sampler_str, frame_id);
            err_cnt++;
        }

        if (err_cnt > 0)
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: frame_id %u : total number of errors %u", _sampler_str, frame_id, err_cnt);
            return false;
        }

        return true;
    }

    void set_first_frame()
    {
        MultiSensorDataBuffer::set_first_frame();
        MultiSensorDataHandler::set_first_frame();

        for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
        {
            _filters[idx].set_first_frame();
        }
    }

    void update_is_more_than_30_fps(bool ans)
    {
        for (uint32_t idx = 0; idx < _num_active_sensors; idx++)
        {
            _filters[idx].update_is_more_than_30_fps(ans);
        }
    }

    int32_t get_interp_filtered(
        uint64_t                        ts_start,
        uint64_t                        ts_end,
        uint32_t                        num_points,
        uint32_t                        sensor_id,
        uint32_t                        frame_id,
        Point2d*                        out,
        SamplerInterpolationStatistics& stats)
    {
        (void)frame_id;

        memset(&stats, 0, sizeof(stats));

        if (false == has_sensor(sensor_id))
        {
            return 1; // nothing to do for this sensor... just make sure to skip the bounding box update ...
        }

        // filter samples vector

        const uint32_t mapped_sensor_id = _sensor_map_inv[sensor_id];

        SamplerInterpolationStatistics stats_x = { 0 };
        SamplerInterpolationStatistics stats_y = { 0 };


        _filters[mapped_sensor_id].get_interp(ts_start, ts_end, frame_id, sensor_id, 0, *this, num_points, out, stats_x, _sampler_str);
        _filters[mapped_sensor_id].get_interp(ts_start, ts_end, frame_id, sensor_id, 1, *this, num_points, out, stats_y, _sampler_str);

        stats.num_error = stats_x.num_error + stats_y.num_error;
        stats.num_exrap_end = stats_x.num_exrap_end + stats_y.num_exrap_end;
        stats.num_exrap_start = stats_x.num_exrap_start + stats_y.num_exrap_start;
        stats.num_processed = MIN(stats_x.num_processed, stats_y.num_processed);


#if defined(_EIS_INTERNAL_DEBUG_)
        {
            IS_ASSERT(NULL != _debug_filter);

            for (uint32_t idx = 0; idx < num_points; idx++)
            {
                const uint64_t ts = timestamp_interpolation(ts_start, ts_end, num_points, idx);
                const Vec3<double> tmp_vec(out[idx]);

                fprintf(_debug_filter, "%u, %" PRIu64 ", %5.15lf, %5.15lf\n",
                    idx, ts, tmp_vec.getX(), tmp_vec.getY());
            }
        }
#endif /* _EIS_INTERNAL_DEBUG_ */


        return 0;
    }
};

class GyroHandler : public SensorDataHandler, public MultiSensorDataBuffer<sample_orientation_t>
{
public:
    rotate_sample*              _rotate_inv;                            /**< Samples rotation inverse function, s.t.
                                                                         *     _rotate(_rotate_inv(sample)) == _rotate_inv(_rotate(sample)) == sample
                                                                         *  For some reason _rotate_inv == _rotate for possible gyro orientations.
                                                                         *  Note that this is not true in the general case! so if changing the method of calculation of _rotate member this also needs to changes
                                                                         */
private: // functions
    // Integrate over time - get theta [rad] from gyro [rad/sec]
    // Using the following formula:
    //          \thata_{n} := \theta_{n-1} + \delta_{n} (g_{n-1} + g_{n}) / 2
    // Where:
    //          \delta_{n} := ((ts_{n} - ts_{n-1}) / 1000000)
    //      since ts_{i} is in [microseconds]. And
    //          g_{i} are in [rad\sec]
    static sample_data_t gyro_samples_integrate(
        const sample_data_t&    prev_sample,
        const sample_data_t&    sample,
        const sample_data_t&    last_theta,
        float                   estimated_frequency)
    {
        (void)estimated_frequency;
        (void)prev_sample;

        //TODO (etsairi): move this block to another function
        // sanity checks
        {
            IS_ASSERT(prev_sample.ts == last_theta.ts);

            IS_ASSERT(prev_sample.ts < sample.ts);

            // strictly monotonic increasing time stamps
            IS_ASSERT(sample.ts > last_theta.ts);

            // delta is smaller then ~35 minutes
            IS_ASSERT(sample.ts <= (0x7FFFFFFF + last_theta.ts));
        }

        const uint32_t t_delta = (uint32_t)(sample.ts - last_theta.ts);

        sample_data_t tmp;

        tmp.ts = sample.ts;
        for (uint32_t a = 0; a < 3; a++)
        {
            tmp.data[a] = last_theta.data[a] + (((sample.data[a] + prev_sample.data[a]) * t_delta) / (2.0 * 1000000));
        }

        return tmp;
    }

    static sample_quat_t gyro_samples_integrate(
        const sample_data_t&    prev_sample,
        const sample_data_t&    sample,
        const sample_quat_t&    last_theta,
        float                   estimated_frequency)
    {
        (void)estimated_frequency;
        (void)prev_sample;

        //TODO (etsairi): move this block to another function
        // sanity checks
        {
            IS_ASSERT(prev_sample.ts == last_theta.ts);

            IS_ASSERT(prev_sample.ts < sample.ts);

            // strictly monotonic increasing time stamps
            IS_ASSERT(sample.ts > last_theta.ts);

            // delta is smaller then ~35 minutes
            IS_ASSERT(sample.ts <= (0x7FFFFFFF + last_theta.ts));
        }

        const uint32_t t_delta = (uint32_t)(sample.ts - last_theta.ts);

        sample_quat_t integrated_quat;

        integrated_quat.ts = sample.ts;

        // center point will be used for integration
        Ang mean_omega;
        for (uint32_t i = 0; i < 3; i++)
        {
            mean_omega(i) =  (sample.data[i] + prev_sample.data[i]) / 2.0;
        }

        //TODO (asimchi): consider integration method
        integrated_quat.quat = Quat::integrate(last_theta.quat, mean_omega, t_delta*1e-6);

        return integrated_quat;
    }


public:  // functions
    GyroHandler(const SensorInitInfo& init, int32_t& status)
    :
        SensorDataHandler(init, "Gyro", false, status),
        MultiSensorDataBuffer(init._num_axis, init._samples_ring_buffer_size, 1, "Gyro", status),
        _rotate_inv(_rotate)                //TODO (etsairi): validate and make sure that this works as expected !
    {};

    ~GyroHandler() {};

    void set_first_frame()
    {
        MultiSensorDataBuffer::set_first_frame();
        SensorDataHandler::set_first_frame();
    }

    static void push_first_to_theta(RingBuffer<sample_data_t>& theta, uint64_t time_stamp)
    {
        sample_data_t first;
        first.ts = time_stamp;
        for (uint32_t i = 0; i < ARR_SIZE(first.data); i++)
        {
            first.data[i] = 0;
        }

        theta.push(first);
    }

    static void push_first_to_theta(RingBuffer<sample_quat_t>& theta, uint64_t time_stamp)
    {
        sample_quat_t first;
        first.ts = time_stamp;
        first.quat = Quat(0, 0, 0, 1);
        theta.push(first);
    }

    // - removes samples from previous frames
    // - rotate sample (according to camera position and sensor mount angle)
    // - integration for fused data: convert [rad/sec] to [rad]
    //
    // returns the number of elements that were processed , negative number for error
    int32_t process_input(const gyro_data_t*  gyro_data, SceneDetection::SceneTypes scene_type)
    {
        IS_ASSERT(NULL != gyro_data);

        if (0 == gyro_data->num_elements)
        {
            return 0; // Nothing to do
        }

        IS_ASSERT(NULL != gyro_data->samples);

#if defined(_EIS_INTERNAL_DEBUG_)
        {
            IS_ASSERT(NULL != _debug_scene_detect);
            fprintf(_debug_scene_detect, "%" PRIu64 ", %u, %u, %u, %u, %f\n",
                gyro_data->samples[0].ts,
                (uint32_t)scene_type & SceneDetection::SceneTypes::SCENE_TYPES_MASK,
                (0 != ((uint32_t)scene_type & SceneDetection::SceneTypes::PANNING_0)),
                (0 != ((uint32_t)scene_type & SceneDetection::SceneTypes::PANNING_1)),
                (0 != ((uint32_t)scene_type & SceneDetection::SceneTypes::PANNING_2)),
                _noise_floor);
        }
#endif /* _EIS_INTERNAL_DEBUG_ */

        uint32_t cnt = 0;

        for (uint32_t idx = 0; idx < gyro_data->num_elements; idx++)
        {
            // Find first relevant sample (removes samples from previous frames)
            if (_last_sample.ts >= gyro_data->samples[idx].ts)
            {
                continue;   /* remove samples from the past */
            }

            // here we use only new gyro data
            cnt++;


            //TODO (etsairi): push the copy into the _rotate function
            sample_data_t rotated = gyro_data->samples[idx];
            _rotate(rotated);     // Rotate gyro sample to agree with camera

            // Act according to scene detection
            // TODO: move this out of here after separating scene detection class from gyro sensor handler
            if (SceneDetection::SceneTypes::TRIPOD == scene_type)
            {
                // treatment for tripod must be before integration not after, since if one frame will not be detected as tripod we will get jump
                rotated.data[0] = 0;
                rotated.data[1] = 0;
                rotated.data[2] = 0;
                rotated.data[3] = 0;
            }
            else
            {
                // subtract the bias from the gyro
                {
                    const sample_data_t bias = BiasEstimatorSingleton::get_accumulated_bias(BiasEstimatorSingleton::instance(NULL));
                    rotated.data[0] -= bias.data[0];
                    rotated.data[1] -= bias.data[1];
                    rotated.data[2] -= bias.data[2];
                    rotated.data[3] -= bias.data[3];
                }
            }

            // Convert gyro to theta
            if (true == _theta.empty())
            {
                push_first_to_theta(_theta, gyro_data->samples[idx].ts);
            }
            else
            {
                _theta.push(gyro_samples_integrate(_last_sample, rotated, _theta.back(), _estimated_frequency));
            }

            _last_sample = rotated;

#if defined(_EIS_INTERNAL_DEBUG_)
            {
                IS_ASSERT(NULL != _debug_process);
                fprintf(_debug_process, "%u, %" PRIu64 ", %5.15lf, %5.15lf, %5.15lf, %5.15lf\n",
                    idx,
                    _last_sample.ts,
                    _last_sample.data[0],
                    _last_sample.data[1],
                    _last_sample.data[2],
                    _last_sample.data[3]);
            }
#endif /* _EIS_INTERNAL_DEBUG_ */
        }

        // Received only old samples, this could indicate on an issue...
        IS_ASSERT((gyro_data->num_elements > 0) && (cnt > 0));

        // _theta is updated with information from latest frame
        return cnt;
    }
};

class IFOVCHandler : public MultiSensorDataHandler, public MultiSensorDataBuffer<sample_data_t>
{
protected: // members
    FilterStateMachine    _state_machine;                               /**< iFOVC internal state machine */
    uint32_t              _buffer_delay;                                /**< buffer delay */
    uint32_t              _last_frame_number_of_samples;
    bool                  _is_more_than_30_fps;

    //Per-sensor information required
    uint32_t                _num_of_sensors_with_ifovc_scaling;           /**< number of sensors with enabled ifovc scale compensation*/
    uint32_t                _map_sensor_idx_to_info[EIS_MAX_NUM_SENSORS]; /**< map between active_sensor_idx to idx in _p_sensors_info*/
    eis_z_lens_position*    _p_sensors_info[EIS_MAX_NUM_SENSORS];         /**< ifovc configuration data per sensor*/

#if defined(_EIS_INTERNAL_DEBUG_)
    FILE*                       _debug_interp_delayed;
    FILE*                       _debug_interp_lut;
#endif /* _EIS_INTERNAL_DEBUG_ */

public:  // functions

    IFOVCHandler(const IFOVCInitInfo& init, int32_t& status)
    :
        MultiSensorDataHandler(init, "iFOVC", status),
        MultiSensorDataBuffer(init._num_axis, init._samples_ring_buffer_size, init._num_active_sensors, "iFOVC", status),
        _state_machine(init._init_common->buffer_delay),
        _buffer_delay(init._init_common->buffer_delay),
        _last_frame_number_of_samples(false),
        _is_more_than_30_fps(false)
    {
        IS_ASSERT(IS_RET_SUCCESS == status);

        uint32_t sensor_idx = 0,info_idx = 0;
        for (; sensor_idx < init._num_sensors; sensor_idx++)
        {
            _map_sensor_idx_to_info[sensor_idx] = info_idx;
            status = copy_eis_z_lens_position(&(init._init_sensor[sensor_idx].is_chromatix_info.z_lens_position),
                                              &(_p_sensors_info[info_idx])); //note that this dynamically allocates the sensor_infos
            info_idx++;
            if (IS_RET_SUCCESS != status)
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Constructor failed");
                break;
            }
        }

#if defined(_EIS_INTERNAL_DEBUG_)
        {
            errno_t err;

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

            {
                char output_csv[1024];
                (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%siFOVC_delayed.csv", g_output_csv_base_folder, INTERPOLATED_PREFIX_FILE_NAME);

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

            {
                char output_csv[1024];
                (void)snprintf(output_csv, ARR_SIZE(output_csv) - 1, "%s/%siFOVC_by_lut.csv", g_output_csv_base_folder, INTERPOLATED_PREFIX_FILE_NAME);

                err = IS_FOPEN(&(_debug_interp_lut), output_csv, "w");
                if (err != 0)
                {
                    IS_ERR(IS_ERR_GENERAL_ERROR, "Failed to open file: %s", output_csv);
                    status = IS_RET_GENERAL_ERROR;
                }
            }
        }
#endif /* _EIS_INTERNAL_DEBUG_ */

        if (status != IS_RET_SUCCESS)
        {
            for (size_t i = 0; i < info_idx; i++)
            {
                _DELETE(_p_sensors_info[i]);
            }
#if defined(_EIS_INTERNAL_DEBUG_)
            IS_FCLOSE(_debug_interp_delayed);
            IS_FCLOSE(_debug_interp_lut);
#endif /* _EIS_INTERNAL_DEBUG_ */
        }
    };

    ~IFOVCHandler()
    {
        for (size_t i = 0; i < _num_sensors; i++)
        {
            _DELETE(_p_sensors_info[i]);
        }
#if defined(_EIS_INTERNAL_DEBUG_)
        IS_FCLOSE(_debug_interp_delayed);
        IS_FCLOSE(_debug_interp_lut);
#endif /* _EIS_INTERNAL_DEBUG_ */
    };

// expected valid data input from IFOVC:
//********************************************************************************//
//                                             +-----------------------+-+        //
//                                          XXX                        |          //
//                                       XXX                           |          //
//                                    XXX                              |          //
//                                 XXX                                 |          //
//                              XXX                                    |          //
//                           XXX                                       |          //
// +--+---------------------+                                          |          //
//    |                                                                |          //
//    |                                                                |          //
//+--------+----------------+------------------+------------------+------> time   //
//    |    +                +                  +                  +    |          //
//    |  sample(s)        additional       additional         sample(s)|          //
//    |  beginning        sample(s) at     sample(s) at       end of   |          //
//    |  of frame         start of         end of             frame    |          //
//    |                   change           change                      |          //
//    |                                                                |          //
//    + <--------------------+frame time+----------------------------> +          //
//                                                                                //
//********************************************************************************//

    bool is_valid(
        const samples_data_t*   samples,
        uint32_t                active_sensor_idx,
        uint32_t                frame_id,
        bool                    is_first_frame,
        bool                    is_sensor_switch)
    {

        (void)is_first_frame;
        (void)frame_id;
        (void)is_sensor_switch;

        const uint32_t mapped_sensor_idx = _sensor_map_inv[active_sensor_idx];
        uint32_t num_errors = 0;

        if (false == has_sensor(active_sensor_idx))
        {
            if ((NULL != samples) && (samples->num_elements > 0))
            {
                IS_ERR(IS_ERR_INVALID_INPUT,
                    "%s: frame_id %u : Sensor #%u is disabled but %u samples were received. Samples will be ignored",
                    _sampler_str, frame_id, active_sensor_idx, samples->num_elements);
            }
        }
        else
        {
            if (NULL == samples)
            {
                IS_ERR(IS_ERR_INVALID_INPUT, "%s : frame_id %u: Sensor #%u is enabled but no samples were received.",
                    _sampler_str, frame_id, active_sensor_idx);
                num_errors++;
            }
            else if(samples->num_elements > 0)
            {
                if (NULL == (samples->samples))
                {
                    IS_ERR(IS_ERR_INVALID_INPUT, "%s : frame_id %u: Sensor #%u Input mismatch, num_elements > 0 but samples array is NULL",
                        _sampler_str, frame_id, active_sensor_idx);
                    num_errors++;
                }
                else
                {
                    bool detectedChange = false;
                    uint32_t newSamplesCnt = 0;
                    uint32_t samplesInRequestedInterval = 0;
                    for (uint32_t idx = 0; idx < samples->num_elements; idx++)
                    {
                        if (idx >= 1) //basic sanity checks: monotonically increasing + duplicated timestamps
                        {
                            const uint64_t t1 = samples->samples[idx - 1].ts;
                            const uint64_t t2 = samples->samples[idx].ts;
                            const sample_data_t* const s1 = &samples->samples[idx - 1];
                            const sample_data_t* const s2 = &samples->samples[idx];

                            if (t1 > t2)//monotonicity
                            {
                                IS_ERR(IS_ERR_SAMPLE_OOO,
                                    "%s: frame_id %u: Time stamps are not strictly monotonically increasing: %" PRIu64 ", %" PRIu64, _sampler_str, frame_id, t1, t2);

                            }
                            else if (t1 == t2) //duplicated samples
                            {
                                IS_ERR(IS_ERR_SAMPLE_DUPLICATED, "%s: frame_id %u: Time stamps are equal, debug is needed : %" PRIu64 "", _sampler_str, frame_id, t1);

                                if (0 != memcmp(s1, s2, sizeof(*s1)))
                                {
                                    IS_ERR(IS_ERR_SAMPLE_DUPLICATED,
                                        "%s: frame_id %u: Time stamps are equal but data samples are not: (%" PRIu64 "; %lf, %lf, %lf), (%" PRIu64 "; %lf, %lf, %lf)",
                                        _sampler_str, frame_id,
                                        s1->ts, s1->data[0], s1->data[1], s1->data[2],
                                        s2->ts, s2->data[0], s2->data[1], s2->data[2]);
                                }
                            }
                            else //basic spacing sanity check
                            {
                                // delta is smaller then ~35 minutes
                                IS_ASSERT(t2 <= (0x7FFFFFFF + t1));
                            }
                        }

                        if (last_requested_interval(active_sensor_idx).first_ts <= samples->samples[idx].ts &&
                            samples->samples[idx].ts <= last_requested_interval(active_sensor_idx).last_ts) // count number of samples in current time interval
                        {
                            samplesInRequestedInterval++;
                            //detect lens position change within frame.
                            if (idx >= 1)
                            {
                                if (samples->samples[idx].data[0] != samples->samples[idx-1].data[0])
                                {
                                    detectedChange = true;
                                }
                            }
                        }

                        //Find new samples and check their validity
                        if (samples->samples[idx].ts > _sensors[mapped_sensor_idx]._last_sample.ts)
                        {
                            newSamplesCnt++;
                            const uint32_t sensor_info_idx = _map_sensor_idx_to_info[active_sensor_idx];
                            if (1 == _p_sensors_info[sensor_info_idx]->scale_source) //use calibrated LUT to get scale
                            {

                                const float maxZLensPosByCalib = _p_sensors_info[sensor_info_idx]->lut_z_lens_pos[0] + (float)IS_EPS;
                                const float minZLensPosByCalib =
                                    _p_sensors_info[sensor_info_idx]->lut_z_lens_pos[(_p_sensors_info[sensor_info_idx]->lut_len) - 1] - (float)IS_EPS;
                                if ((samples->samples[idx].data[0] < minZLensPosByCalib) || (samples->samples[idx].data[0] > maxZLensPosByCalib))
                                {

                                    IS_ERR(IS_ERR_GENERAL_WARNING, "%s : frame_id %u: Sensor #%u Invalid input,"
                                        " IFOVC lens position is out of the calibrated range [%f,%f]: %f",
                                        _sampler_str, frame_id, active_sensor_idx,
                                        minZLensPosByCalib, maxZLensPosByCalib, samples->samples[idx].data[0]);
                                }
                            }
                            else //using AF data to get scale
                            {
                                if (samples->samples[idx].data[1] <= 0)
                                {
                                    IS_ERR(IS_ERR_INVALID_INPUT, "%s : frame_id %u: Sensor #%u invalid input, scale factor value is below 0: %f",
                                        _sampler_str, frame_id, active_sensor_idx, samples->samples[idx].data[1]);
                                    num_errors++;
                                }
                                if (samples->samples[idx].data[1] > 1.0)
                                {
                                    IS_HIGH(IS_ERR_GENERAL_WARNING, "%s : frame_id %u: Sensor #%u invalid input, scale factor value is larger than 1: %f",
                                        _sampler_str, frame_id, active_sensor_idx, samples->samples[idx].data[1]);
                                }
                            }
                        }
                    }

                    const bool validateNumOfSamples = (false == _is_more_than_30_fps ||
                        (true == _is_more_than_30_fps && _last_frame_number_of_samples < IFOVC_MIN_SAMPLES_PER_FRAME)); //for FPS higher than 30,
                                                                                               // samples are replicated. e.g. in 60 FPS,
                                                                                               //IFOVC samples for frame #n and frame #(n+1) are the same.
                                                                                               //thus, need to validate number of samples only if there were none on last frame.
                    if (true == validateNumOfSamples)
                    {
                        //Validate number of new samples
                        if (0 == newSamplesCnt)
                        {
                            IS_HIGH(IS_ERR_SAMPLER_MISSING_SAMPLE,
                                "%s: frame_id %u: Bad input - Did not receive new samples: "
                                "last sample timestamp %" PRIu64 " ; "
                                "last sample timestamp from input %" PRIu64 " ; "
                                "input number of elements %u. ",
                                _sampler_str, frame_id,
                                _sensors[mapped_sensor_idx]._last_sample.ts,
                                samples->samples[samples->num_elements - 1].ts,
                                samples->num_elements);
                        }

                        // Validate number of samples in requested time interval
                        if (samplesInRequestedInterval == 0)
                        {
                            IS_ERR(IS_ERR_INVALID_INPUT, "%s : frame_id %u: Sensor #%u has 0 IFOVC samples. "
                                ". Requested Interval: [%" PRIu64 ",%" PRIu64 "]",
                                _sampler_str, frame_id, active_sensor_idx,
                                last_requested_interval(active_sensor_idx).first_ts,
                                last_requested_interval(active_sensor_idx).last_ts);
                            if (0 == _last_frame_number_of_samples)
                            {
                                IS_HIGH(IS_ERR_GENERAL_WARNING, "%s : frame_id %u: Sensor #%u has two consecutive frames without data."
                                    "Suspecting end of stream.",
                                    _sampler_str, frame_id, active_sensor_idx);
                            }
                        }

                        if (0 < samplesInRequestedInterval && samplesInRequestedInterval < IFOVC_MIN_SAMPLES_PER_FRAME)
                        {
                            IS_HIGH(IS_ERR_SAMPLER_MISSING_SAMPLE, "%s : frame_id %u: Sensor #%u low number of IFOVC samples. "
                                "Number of samples: %" PRIu32 ", expected: %u . Requested Interval: [%" PRIu64 ",%" PRIu64 "]",
                                _sampler_str, frame_id, active_sensor_idx, samplesInRequestedInterval,
                                IFOVC_MIN_SAMPLES_PER_FRAME,
                                last_requested_interval(active_sensor_idx).first_ts,
                                last_requested_interval(active_sensor_idx).last_ts);
                        }

                        if (true == detectedChange)
                        {
                            if (samplesInRequestedInterval < IFOVC_MIN_SAMPLES_PER_FRAME_WHILE_CHANGE)
                            {
                                IS_HIGH(IS_ERR_SAMPLER_MISSING_SAMPLE, "%s : frame_id %u : Sensor #%u low number IFOVC samples "
                                    "during data change. Number of samples: %u, Expected: %u ",
                                    _sampler_str, frame_id, active_sensor_idx, samplesInRequestedInterval, IFOVC_MIN_SAMPLES_PER_FRAME_WHILE_CHANGE);
                            }
                        }
                    }
                    _last_frame_number_of_samples = samplesInRequestedInterval;
                }
            }
        }

        if (num_errors > 0)
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "%s: frame_id %u : Invalid data from sensor #%u",
                _sampler_str, frame_id, active_sensor_idx);
            return false;
        }

        return true;
    }

    void set_first_frame()
    {
        MultiSensorDataBuffer::set_first_frame();
        MultiSensorDataHandler::set_first_frame();

        _state_machine.set_first_frame();
    }

    int32_t process_input(
        const ifovc_data_t*     ifovc_data,
        uint32_t                active_sensor_idx,
        int32_t                 ifovc_offset,
        const Camera&           camera,
        const ZoomWindows&      frame_zoom)
    {
        IS_ASSERT(NULL != ifovc_data);

        if (false == has_sensor(active_sensor_idx))
        {
            // Either way update the internal state machine since it must be aligned
            // with the number of frames that were processed
            if (IS_RET_SUCCESS != _state_machine.update())
            {
                IS_ERR(IS_ERR_GENERAL_ERROR, "Failed to updated iFOVC state machine");
                return -2;
            }

            return 0;  // Nothing to do ... sampler is disabled
        }

        if (ifovc_data->num_elements == 0)
        {
            return 0; // Nothing to do ...
        }

        IS_ASSERT(NULL != ifovc_data->samples);

        uint32_t cnt = 0;
        const uint32_t mapped_sensor_idx = _sensor_map_inv[active_sensor_idx];

        for (uint32_t idx = 0; idx < ifovc_data->num_elements; idx++)
        {
            // Find first relevant sample (removes samples from previous frames)
            if (_sensors[mapped_sensor_idx]._last_sample.ts >= ifovc_data->samples[idx].ts)
            {
                continue;   /* remove samples from the past */
            }

            if ((true == isnan(ifovc_data->samples[idx].data[0])) ||
                (true == isnan(ifovc_data->samples[idx].data[1])))
            {
                IS_ERR(IS_ERR_INVALID_INIT_INPUT, "IFOVC: Invalid (NaN) sample at index: %u, check issue from AF IFOVC. sample is ignored", idx);
                continue;
            }

            // here we use only new iFOVC data
            cnt++;

            sample_data_t ifovc_sample = ifovc_data->samples[idx];
            const uint64_t ts_backup = ifovc_sample.ts;

            IS_ASSERT(ifovc_sample.ts > (uint64_t)MAX(0, ifovc_offset));  // Sanity check

            //TODO (etsairi): is this needed?
            // Ask Ron
            //_sensors[mapped_sensor_idx]._rotate(ifovc_sample);         // Rotate the iFOVC sample so it will agree with the gyro sub-system. not gyro at all, maybe agree with image

            adjust_ifovc(camera, frame_zoom, ifovc_sample);

            //ifovc_sample.ts -= ifovc_offset; fix UBSAN error
            ifovc_sample.ts = (uint64_t)((int64_t)ifovc_sample.ts - (int64_t)ifovc_offset);
            push_data(ifovc_sample, active_sensor_idx);

            IS_ASSERT((NULL != _sensor_id) == (_num_active_sensors > 1));

            _sensors[mapped_sensor_idx]._last_sample = ifovc_sample;
            _sensors[mapped_sensor_idx]._last_sample.ts = ts_backup;  // actual iFOVC ts

#if defined(_EIS_INTERNAL_DEBUG_)
            {
                IS_ASSERT(NULL != _debug_process);
                fprintf(_debug_process, "%u, %" PRIu64 ", %5.15lf, %5.15lf\n",
                    idx,
                    _sensors[mapped_sensor_idx]._last_sample.ts,
                    _sensors[mapped_sensor_idx]._last_sample.data[0],
                    _sensors[mapped_sensor_idx]._last_sample.data[1]);
            }
#endif /* _EIS_INTERNAL_DEBUG_ */

        }

        if (IS_RET_SUCCESS != _state_machine.update())
        {
            IS_ERR(IS_ERR_GENERAL_ERROR, "Failed to updated iFOVC state machine");
            return -1;
        }

        // Received only old samples, this could indicate on an issue...
        IS_ASSERT((ifovc_data->num_elements > 0));

        return cnt;
    }

    int32_t get_interp_delayed(
        uint64_t                        ts_start,
        uint64_t                        ts_end,
        uint32_t                        num_points,
        uint32_t                        sensor_id,
        double*                         out,
        SamplerInterpolationStatistics& stats)
    {
        const FilterStateMachine::Action action = _state_machine.set_action();

        memset(&stats, 0, sizeof(stats));

        if (FilterStateMachine::DO_NOTHING == action)
        {
            return 1;
        }
        else
        {
            if (false == has_sensor(sensor_id))
            {
                return 1; // nothing to do for this sensor... just make sure to skip the bounding box update ...
            }

            Point2d tmp[27];

            IS_ASSERT(ARR_SIZE(tmp) == num_points);

            MultiSensorDataBuffer::get_interp(ts_start, ts_end, num_points, sensor_id, 0, false, tmp, stats, false);

            for (uint32_t idx = 0; idx < num_points; idx++)
            {
                out[idx] = tmp[idx].at(1);
            }

#if defined(_EIS_INTERNAL_DEBUG_)
            {
                IS_ASSERT(NULL != _debug_interp_delayed);

                for (uint32_t idx = 0; idx < num_points; idx++)
                {
                    const uint64_t ts = timestamp_interpolation(ts_start, ts_end, num_points, idx);
                    fprintf(_debug_interp_delayed, "%u, %" PRIu64 ", %5.15lf\n", idx, ts, out[idx]);
                }
            }
#endif /* _EIS_INTERNAL_DEBUG_ */

            return 0;
        }
    }

    // get z_lens_pos and converts to scale
    double convert_lens_pos_to_scale(const uint32_t frame_id,
                                     const uint64_t ts_start,
                                     const uint64_t ts_end,
                                     const uint32_t num_points,
                                     const uint32_t idx,
                                     const double z_lens_pos,
                                     const uint32_t active_sensor_idx)
    {


        double scale = 1;
        const size_t sensor_info_idx = _map_sensor_idx_to_info[active_sensor_idx];
        const eis_z_lens_position* curSensorIfovcInfo = _p_sensors_info[sensor_info_idx];

        IS_ASSERT(curSensorIfovcInfo->scale_source == 1);
        IS_ASSERT(curSensorIfovcInfo->lut_len > 1);

        if (curSensorIfovcInfo->enable)
        {
            scale = interp_scale(curSensorIfovcInfo->lut_z_lens_pos,
                                 curSensorIfovcInfo->lut_scale_vs_z_lens_pos,
                                 curSensorIfovcInfo->lut_len,
                                 z_lens_pos);
            scale = MAX(curSensorIfovcInfo->min_scale_limit, scale);
        }
        else
        {
            scale = 1;
        }

#ifdef _EIS_INTERNAL_DEBUG_
        IS_ASSERT(NULL != _debug_interp_lut);
        const uint64_t ts = timestamp_interpolation(ts_start, ts_end,num_points,idx);
        fprintf(_debug_interp_lut, "%u, %u, %" PRIu64 ", %5.15f, %5.15lf\n",frame_id, idx, ts, z_lens_pos,scale);
#else
        (void)frame_id;
        (void)ts_start;
        (void)ts_end;
        (void)num_points;
        (void)idx;
#endif // _EIS_INTERNAL_DEBUG_

        return scale;
    }

    uint32_t get_scale_source(uint32_t sensor_idx) {
        IS_ASSERT(sensor_idx < this->_num_active_sensors);
        uint32_t sensor_info_idx = _map_sensor_idx_to_info[sensor_idx];
        return _p_sensors_info[sensor_info_idx]->scale_source;
    }

    bool has_sensor(const uint32_t sensor_idx) const
    {
        const bool ifovcEnable = MultiSensorDataHandler::has_sensor(sensor_idx);
        const bool z_lens_pos_enabled = (1 == _p_sensors_info[_map_sensor_idx_to_info[sensor_idx]]->enable);
        return ifovcEnable && z_lens_pos_enabled;
    }

    void update_is_more_than_30_fps(bool ans)
    {
        _is_more_than_30_fps = ans;
    }

private: // functions
    static void adjust_ifovc(const Camera& camera, const ZoomWindows&  frame_zoom, sample_data_t& sample)
    {
        (void)camera;
        (void)frame_zoom;
        (void)sample;

        //Nothing to do here.
    }

    static double interp_scale(const float* lensPosLut, const float* scaleLut, const uint32_t lutLen, const double curLensPos) {
        double res = 1;
        if (curLensPos > lensPosLut[0]) //extrapolate over by ZOH
        {
            res = scaleLut[lutLen - 1];
        }
        else if (curLensPos < lensPosLut[lutLen - 1]) //extrapolate under by ZOH
        {
            res = scaleLut[0];
        }
        else //interpolate
        {
            size_t i = 1;
            for (; i < lutLen; i++)
            {
                if (lensPosLut[i - 1] >= curLensPos && curLensPos >= lensPosLut[i])
                {
                    break;
                }
            }
            IS_ASSERT(lensPosLut[i-1] - lensPosLut[i] > IS_EPS);
            const double w = (lensPosLut[i - 1] - curLensPos) / (lensPosLut[i-1] - lensPosLut[i]);
            res = w * scaleLut[i] + (1 - w)*scaleLut[i-1];
        }
        return res;
    }

    static int32_t copy_eis_z_lens_position(const eis_z_lens_position* src, eis_z_lens_position** dst) {
        if (NULL == src || NULL == dst) {
            IS_ERR(IS_ERR_GENERAL_ERROR, "Copy To NULL requested");
            return IS_RET_GENERAL_ERROR;

        }
        *dst = new(std::nothrow) eis_z_lens_position(*src); //note that this should be a deep copy by C standards.
        if (NULL == *dst)
        {
            IS_ERR(IS_ERR_OUT_OF_MEMORY, "out of memory");
            return IS_RET_OUT_OF_MEMORY;
        }

        for (size_t i = 0; i < src->lut_len; i++) //assert that deep copy happened.
        {
            IS_ASSERT(src->lut_scale_vs_z_lens_pos[i] == (*dst)->lut_scale_vs_z_lens_pos[i]);
            IS_ASSERT(src->lut_z_lens_pos[i] == (*dst)->lut_z_lens_pos[i]);
        }

        return IS_RET_SUCCESS;
    }
};
