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

#pragma once

#include <cmath>

#include "zoomwindows.h"
#include "datatypes.h"
#include "sensordata.h"


//#define __DEBUG_CAMERA_MATRIX__

// works like expected
class Camera //TODO (etsairi): change to agree with file name !!!
{
public:
    double fx;      /// Focal length on x-axis [pixels]
    double fy;      /// Focal length on y-axis [pixels]
    double cx;      /// Camera center w.r.t. x-axis [pixels]
    double cy;      /// Camera center w.r.t. y-axis [pixels]

    uint32_t im_w;  /// Input image width
    uint32_t im_h;  /// Input image height

    double crop_w;  /// Output image crop factor x-axis
    double crop_h;  /// Output image crop factor y-axis

    //TODO (etsairi): Add LDC grid member and function support
    //TODO (etsairi): Add out_w and out_h

#ifdef __DEBUG_CAMERA_MATRIX__

    //TODO (etsairi): remove if not needed
    Hom angle_to_homography(const Ang &angle, const Mat &K, bool swap) const
    {
        Mat Rx = { 1, 0, 0, 0, cos(angle(0)), -sin(angle(0)), 0, sin(angle(0)), cos(angle(0)) };
        Mat Ry = { cos(angle(1)), 0, sin(angle(1)), 0, 1, 0, -sin(angle(1)), 0, cos(angle(1)) };
        Mat Rz = { cos(angle(2)), -sin(angle(2)), 0, sin(angle(2)), cos(angle(2)), 0, 0, 0, 1 };
        Mat R;
        if (swap)
            R = Rx * Ry * Rz;
        else
            R = Rz * Ry * Rx;

        return K * R / K;
    }

    // camera.angle_to_homography_ron(angle, zoom) == camera.rot(angle, zoom);
    Hom angle_to_homography_ron(const Ang &angle, const double input_scale) const
    {
        const double ca0 = cos(angle(0)), sa0 = sin(angle(0));
        const double ca1 = cos(angle(1)), sa1 = sin(angle(1));
        const double ca2 = cos(angle(2)), sa2 = sin(angle(2));

        Mat Rx = { 1, 0, 0,        0,  ca1, sa0,   0, -sa0, ca0 };
        Mat Ry = { ca1, 0, -sa1,   0,   1,   0 ,  sa1,  0,  ca1 };
        Mat Rz = { ca2, sa2, 0,  -sa2, ca2,  0 ,   0,   0,   1 };

        /*
        Mat Rx = { 1, 0, 0, 0, cos(angle(0)), sin(angle(0)), 0, -sin(angle(0)), cos(angle(0)) };
        Mat Ry = { cos(angle(1)), 0, -sin(angle(1)), 0, 1, 0, sin(angle(1)), 0, cos(angle(1)) };
        Mat Rz = { cos(angle(2)), sin(angle(2)), 0, -sin(angle(2)), cos(angle(2)), 0, 0, 0, 1 };
        */

        Mat R;

        R = Rx * Ry * Rz;

        const double fxz = fx / input_scale;
        const double fyz = fy / input_scale;

        Mat K = Mat(fxz, 0, cx, 0, fyz, cy, 0, 0, 1);
        return K * R / K;
    }


#endif /* __DEBUG_CAMERA_MATRIX__ */

public:
    ~Camera() {}

    //TODO (etsairi): remove when possible!
    Camera() :
        fx(NAN),
        fy(NAN),
        cx(NAN),
        cy(NAN),
        im_w(0),
        im_h(0),
        crop_w(NAN),
        crop_h(NAN)
    {}

    //TODO (etsairi): document!!!
    //                Note: cx_ and cy_ assumes that origin is located at top-left corner !!!
    Camera(double fx_, double fy_, uint32_t im_w_, uint32_t im_h_, double cx_, double cy_, double crop_w_, double crop_h_) :
        fx(fx_),
        fy(fy_),
        cx(cx_),
        cy(cy_),
        im_w(im_w_),
        im_h(im_h_),
        crop_w(crop_w_),
        crop_h(crop_h_)
    {
        IS_ASSERT(im_h > 0);
        IS_ASSERT(im_w > 0);
    }

    inline Hom to_mat() const
    {
        return Hom(fx, 0, cx, 0, fy, cy, 0, 0, 1);
    }

    inline Hom to_mat_inv() const
    {
        return Hom(1/fx, 0, -cx/fx, 0, 1/fy, -cy/fy, 0, 0, 1);
    }

    //TODO (etsairi): profile this code, see how much it saves in if negligible then replace with naive (and readable) implementation
    inline Hom rot(const Ang &ang, const ZoomWindows &zoom) const
    {
        //equivalent to:
        // angle = dst - src
        // R = R(angle)
        // H = K * (Rz * Ry * Rx) * K^-1
        // R defined in:
        // www.en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations

        // output: is rotation matrix from "src" to "dst"


        // below code is original function and current implementation is optimized one.
        //
        //      const double ca0 = cos(angle(0)), sa0 = sin(angle(0));
        //      const double ca1 = cos(angle(1)), sa1 = sin(angle(1));
        //      const double ca2 = cos(angle(2)), sa2 = sin(angle(2));
        //
        //      Mat Rx = { 1, 0, 0, 0, ca1, sa0, 0, -sa0, ca0 };
        //      Mat Ry = { ca1, 0, -sa1, 0, 1, 0 , sa1, 0, ca1 };
        //      Mat Rz = { ca2, sa2, 0, -sa2, ca2, 0 , 0, 0, 1 };
        //
        //      /*
        //      Mat Rx = { 1, 0, 0, 0, cos(angle(0)), sin(angle(0)), 0, -sin(angle(0)), cos(angle(0)) };
        //      Mat Ry = { cos(angle(1)), 0, -sin(angle(1)), 0, 1, 0, sin(angle(1)), 0, cos(angle(1)) };
        //      Mat Rz = { cos(angle(2)), sin(angle(2)), 0, -sin(angle(2)), cos(angle(2)), 0, 0, 0, 1 };
        //      */
        //
        //      Mat R;
        //
        //      R = Rx * Ry * Rz;
        //
        //      const double fxz = fx / input_scale;
        //      const double fyz = fy / input_scale;
        //
        //      Mat K = Mat(fxz, 0, cx, 0, fyz, cy, 0, 0, 1);
        //      return K * R / K;


        const double ca1 = cos(ang(0)), sa1 = sin(ang(0));
        const double ca2 = cos(ang(1)), sa2 = sin(ang(1));
        const double ca3 = cos(ang(2)), sa3 = sin(ang(2));
        const double fxz = fx / zoom.input_scale;
        const double fyz = fy / zoom.input_scale;
        const double ifx = 1.0 / fxz;
        const double ify = 1.0 / fyz;
        const double ifxy = ifx * ify;

        const Hom res = Hom(
            (ca2*ca3*fxz + cx*sa1*sa3 + ca1*ca3*cx*sa2)*ifx,                    (ca2*fxz*sa3 - ca3*cx*sa1 + ca1*cx*sa2*sa3)*ify,                   -(fxz*fxz*fyz*sa2 + ca2*cy*fxz*fxz*sa3 + cx*cx*fyz*sa1*sa3 - ca1*ca2*cx*fxz*fyz + ca2*ca3*cx*fxz*fyz - ca3*cx*cy*fxz*sa1 + ca1*ca3*cx*cx*fyz*sa2 + ca1*cx*cy*fxz*sa2*sa3)*ifxy,
            (cy*sa1*sa3 - ca1*fyz*sa3 + ca3*fyz*sa1*sa2 + ca1*ca3*cy*sa2)*ifx,  (ca1*ca3*fyz - ca3*cy*sa1 + ca1*cy*sa2*sa3 + fyz*sa1*sa2*sa3)*ify, -(ca1*ca3*cy*fxz*fyz - ca3*cy*cy*fxz*sa1 - ca2*fxz*fyz*fyz*sa1 - ca1*ca2*cy*fxz*fyz - ca1*cx*fyz*fyz*sa3 + cx*cy*fyz*sa1*sa3 + ca1*cy*cy*fxz*sa2*sa3 + ca3*cx*fyz*fyz*sa1*sa2 + ca1*ca3*cx*cy*fyz*sa2 + cy*fxz*fyz*sa1*sa2*sa3)*ifxy,
            (sa1*sa3 + ca1*ca3*sa2)*ifx,                                       -(ca3*sa1 - ca1*sa2*sa3)*ify,                                       -(cx*fyz*sa1*sa3 - ca3*cy*fxz*sa1 - ca1*ca2*fxz*fyz + ca1*ca3*cx*fyz*sa2 + ca1*cy*fxz*sa2*sa3)*ifxy
        );
        return res;
    }

    //TODO (etsairi): profile this code, see how much it saves in if negligible then replace with naive (and readable) implementation
    inline Hom rot_inv(const Ang &ang, const ZoomWindows &zoom) const
    {
        // Note:  rot(- ang) != rot_inv(ang)

        //equivalent to:
        // angle = dst - src
        // R = R(-angle)
        // H = K * (Rx * Ry * Rz) * K^-1
        // R defined in:
        // www.en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations

        const double ca1 = cos(ang(0)), sa1 = sin(ang(0));
        const double ca2 = cos(ang(1)), sa2 = sin(ang(1));
        const double ca3 = cos(ang(2)), sa3 = sin(ang(2));
        const double fxz = fx / zoom.input_scale;
        const double fyz = fy / zoom.input_scale;
        const double ifx = 1.0 / fxz;
        const double ify = 1.0 / fyz;
        const double ifxy = ifx * ify;

        const Hom res = Hom(
           -(cx*sa2 - ca2*ca3*fxz)*ifx, (ca2*cx*sa1 - ca1*fxz*sa3 + ca3*fxz*sa1*sa2)*ify,  (cx*cx*fyz*sa2 + ca1*cy*fxz*fxz*sa3 + fxz*fxz*fyz*sa1*sa3 + ca1*ca2*cx*fxz*fyz - ca2*ca3*cx*fxz*fyz - ca2*cx*cy*fxz*sa1 + ca1*ca3*fxz*fxz*fyz*sa2 - ca3*cy*fxz*fxz*sa1*sa2)*ifxy,
           -(cy*sa2 - ca2*fyz*sa3)*ifx, (ca1*ca3*fyz + ca2*cy*sa1 + fyz*sa1*sa2*sa3)*ify, -(ca2*cy*cy*fxz*sa1 - cx*cy*fyz*sa2 + ca2*cx*fyz*fyz*sa3 + ca3*fxz*fyz*fyz*sa1 - ca1*ca2*cy*fxz*fyz + ca1*ca3*cy*fxz*fyz - ca1*fxz*fyz*fyz*sa2*sa3 + cy*fxz*fyz*sa1*sa2*sa3)*ifxy,
            -sa2*ifx,                   (ca2*sa1)*ify,                                     (cx*fyz*sa2 - ca2*cy*fxz*sa1 + ca1*ca2*fxz*fyz)*ifxy
        );
        return res;
    }

    inline Hom rot(const Quat &quat, const ZoomWindows &zoom) const
    {
        const double fxz = fx / zoom.input_scale;
        const double fyz = fy / zoom.input_scale;
        const double ifx = 1.0 / fxz;
        const double ify = 1.0 / fyz;

        Hom R = (quat.inv()).to_mat();

        Hom res = {
            R(2,0)*cx*ifx + R(0,0), (R(2,1)*cx + R(0,1)*fxz)*ify, R(2,2)*cx + R(0,2)*fxz - (cx*(R(2,0)*cx*ifx + R(0,0))) - (cy*(R(2,1)*cx + R(0,1)*fxz))*ify,
            (R(2,0)*cy + R(1,0)*fyz)*ifx, (R(2,1)*cy*ify + R(1,1)), R(2,2)*cy + R(1,2)*fyz - (cx*(R(2,0)*cy + R(1,0)*fyz))*ifx - (cy*(R(2,1)*cy*ify + R(1,1))),
            R(2,0)*ifx, R(2,1)*ify, R(2,2) - (R(2,0)*cx)*ifx - (R(2,1)*cy)*ify };

        return res;
    }

    inline Hom rot_inv(const Quat &quat, const ZoomWindows &zoom) const
    {
        const double fxz = fx / zoom.input_scale;
        const double fyz = fy / zoom.input_scale;
        const double ifx = 1.0 / fxz;
        const double ify = 1.0 / fyz;

        Hom R = quat.to_mat();

        Hom res = {
            R(2,0)*cx*ifx + R(0,0), (R(2,1)*cx + R(0,1)*fxz)*ify, R(2,2)*cx + R(0,2)*fxz - (cx*(R(2,0)*cx*ifx + R(0,0))) - (cy*(R(2,1)*cx + R(0,1)*fxz))*ify,
            (R(2,0)*cy + R(1,0)*fyz)*ifx, (R(2,1)*cy*ify + R(1,1)), R(2,2)*cy + R(1,2)*fyz - (cx*(R(2,0)*cy + R(1,0)*fyz))*ifx - (cy*(R(2,1)*cy*ify + R(1,1))),
            R(2,0)*ifx, R(2,1)*ify, R(2,2) - (R(2,0)*cx)*ifx - (R(2,1)*cy)*ify };

        return res;
    }

    inline Point2d unproject(const Point2d & src, const ZoomWindows &zoom) const
    {
        Point2d res;

        //TODO (etsairi): what about optical center? need to check

        const double fxz = fx / zoom.input_scale;
        const double fyz = fy / zoom.input_scale;

        res.x = (src.x - cx) / fxz;
        res.y = (src.y - cy) / fyz;
        return res;
    }

    inline Point2d project(const Point2d & src, const ZoomWindows &zoom) const
    {
        Point2d res;

        const double fxz = fx / zoom.input_scale;
        const double fyz = fy / zoom.input_scale;

        res.x = src.x*fxz+cx;
        res.y = src.y*fyz+cy;
        return res;
    }

    //TODO (etsairi): profile this code, see how much it saves in if negligible then replace with naive (and readable) implementation
    static inline Hom rot_from_to(
        const Ang&          from,
        const Ang&          to,
        const Camera&       cam_from,
        const Camera&       cam_to,
        const ZoomWindows&  zoom_from,
        const ZoomWindows&  zoom_to)
    {
        //equivalent to:
        // angle = dst - src
        // R = R(angle)
        // H = K * (Rz * Ry * Rx) * K^-1
        // R defined in:
        // www.en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations

        // output: is rotation matrix from "src" to "dst"
       // Ang angle = angleTo - angleFrom;
        const Ang angle = Ang::difference(from, to);

        const double ca0 = cos(angle(0)), sa0 = sin(angle(0));
        const double ca1 = cos(angle(1)), sa1 = sin(angle(1));
        const double ca2 = cos(angle(2)), sa2 = sin(angle(2));

        const Mat Rx = { 1, 0, 0, 0, ca1, sa0, 0, -sa0, ca0 };
        const Mat Ry = { ca1, 0, -sa1, 0, 1, 0 , sa1, 0, ca1 };
        const Mat Rz = { ca2, sa2, 0, -sa2, ca2, 0 , 0, 0, 1 };
        const Hom R = Rx * Ry * Rz;

        // TODO(rgaizman): treat general case of crop not centered before eis by mult with matrix on the right and left of entire res , the matrices contain scaling and shift accordingly
        Mat Kfrom = Mat(cam_from.fx / zoom_from.input_scale,    0,                                      cam_from.cx,
                        0,                                      cam_from.fy / zoom_from.input_scale,    cam_from.cy,
                        0,                                      0,                                      1);

        Mat Kto = Mat(cam_to.fx / zoom_to.input_scale,          0,                                      cam_to.cx,
                      0,                                        cam_to.fy / zoom_to.input_scale,        cam_to.cy,
                      0,                                        0,                                      1);

        const Hom res = Kto * R / Kfrom;

        return res;
    }
 //
 // //TODO (etsairi): profile this code, see how much it saves in if negligible then replace with naive (and readable) implementation
 // static inline Hom rot_from_to(const Quat &angleFrom, const Quat &angleTo, const Camera& camMatrixFrom, const Camera& camMatrixTo, const ZoomWindows &zoomFrom, const ZoomWindows &zoomTo)
 // {
 //     //equivalent to:
 //     // angle = dst - src
 //     // R = R(angle)
 //     // H = K * (Rz * Ry * Rx) * K^-1
 //     // R defined in:
 //     // www.en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
 //
 //     // output: is rotation matrix from "src" to "dst"
 //     // Ang angle = angleTo - angleFrom;
 //     const Orientation angle = Orientation::difference(angleFrom, angleTo);
 //
 //     const Hom R = (angle.inv()).to_mat();
 //     // TODO (rgaizman): Quat are inversed, all integration and usage is inversed , check maybe inverse all and then can use here the quat instead of quat.inv
 //     // then maybe can use same function for quat and ang, if both derive from orientaiton instead of defining it
 // }
 //
 // // TODO(rgaizman): treat general case of crop not centered before eis by mult with matrix on the right and left of entire res , the matrices contain scaling and shift accordingly
 // Mat Kfrom = Mat(camMatrixFrom.fx / zoomFrom.input_scale, 0, camMatrixFrom.cx,
 //     0, camMatrixFrom.fy / zoomFrom.input_scale, camMatrixFrom.cy,
 //     0, 0, 1);
 // Mat Kto = Mat(camMatrixTo.fx / zoomTo.input_scale, 0, camMatrixTo.cx,
 //     0, camMatrixTo.fy / zoomTo.input_scale, camMatrixTo.cy,
 //     0, 0, 1);
 //
 // const Hom res = Kto * R / Kfrom;
 //
 // return res;
 // }
};

inline Ang process_gimbal_ois(  Ang ois_sample_tmp,
                                SensorData::ois_module_type_t ois_module_type,
                                const double ois_data_conversion_x[ARR_SIZE(eis_ois::data_conversion_x)],
                                const double ois_data_conversion_y[ARR_SIZE(eis_ois::data_conversion_y)] )
{
    Ang ois_sample_rot(0, 0, 0);

    if (SensorData::GIMBAL_PRISM == ois_module_type)
    {
        // For SS OIS prism, this works along with ois lens orientaton of 1 2 3
        ois_sample_rot(0) = -( ois_sample_tmp(1) * ois_data_conversion_x[1] + POW3(ois_sample_tmp(1)) * ois_data_conversion_x[6] ) * IS_PI / 180 ;
        ois_sample_rot(1) = -( ois_sample_tmp(0) * ois_data_conversion_y[2] + POW3(ois_sample_tmp(0)) * ois_data_conversion_y[9] ) * IS_PI / 180 ;

        ois_sample_rot(2) = -ois_sample_rot(1);
    }
    else if (SensorData::GIMBAL == ois_module_type)
    {
        // TODO (rgaizman): need to get convertion factor for z if needed
        ois_sample_rot(0) = DEG_TO_RAD( ois_sample_tmp(0) * ois_data_conversion_x[1] + POW3(ois_sample_tmp(0)) * ois_data_conversion_x[6] ) ;
        ois_sample_rot(1) = DEG_TO_RAD( ois_sample_tmp(1) * ois_data_conversion_y[2] + POW3(ois_sample_tmp(1)) * ois_data_conversion_y[9] ) ;
        ois_sample_rot(2) = DEG_TO_RAD( ois_sample_tmp(2) * ois_data_conversion_y[2] + POW3(ois_sample_tmp(2)) * ois_data_conversion_y[9] ) ; // TODO (rgaizman): get it coeff by its own


     //  ois_sample_rot(0) = ( ois_sample_tmp(0) * ois_data_conversion_x[1] + POW3(ois_sample_tmp(0)) * ois_data_conversion_x[6] ) * IS_PI / 180 ;
     //  ois_sample_rot(1) = ( ois_sample_tmp(1) * ois_data_conversion_y[2] + POW3(ois_sample_tmp(1)) * ois_data_conversion_y[9] ) * IS_PI / 180;
     //  ois_sample_rot(2) = ( ois_sample_tmp(2) * ois_data_conversion_y[2] + POW3(ois_sample_tmp(2)) * ois_data_conversion_y[9] ) * IS_PI / 180 ; // TODO (rgaizman): get it coeff by its own
    }
    else if (SensorData::GIMBAL_PRISM_Y_LENS_SHIFT_X == ois_module_type)
    {
        // Trerating the part for x axis rotation (y shift).
        // Other part of x axis lens shift is treated separately
        ois_sample_rot(0) = -DEG_TO_RAD( ois_sample_tmp(1) * ois_data_conversion_y[2] + POW3(ois_sample_tmp(1)) * ois_data_conversion_y[9] );
        ois_sample_rot(1) = 0;
        ois_sample_rot(2) = 0;
    }
    else
    {
        IS_ERR(IS_ERR_GENERAL_ERROR, "Misusage of function.");
        IS_ASSERT(0);
    }

    return ois_sample_rot;
}

class OISOperator
{
protected:
    const Point2d _raw_offset; /** Raw offset to be added to OIS samples */

public:
    typedef SensorData::ois_module_type_t   ModuleType;

    OISOperator(const is_init_data_sensor&  sensor)
    :
        _raw_offset(sensor.is_chromatix_info.ois__raw_offset, sensor.is_chromatix_info.ois__raw_offset)
    {
        IS_ASSERT(sensor.is_chromatix_info.top.ois_frequency > 0);
    }

    virtual ~OISOperator() {}
    virtual void update(const Camera& camera, const ZoomWindows& zoom) = 0;
    virtual Ang get_average_angle_o2i(const Point2d* ois_samples, uint32_t num_samples) const = 0;
    virtual Hom get_projectiv_o2i(Point2d ois) const = 0;
    virtual inline Point2d apply_on_point_o2i(Point2d point, Point2d ois) const = 0;
    virtual inline Point2d apply_on_point_i2o(Point2d point, Point2d ois) const = 0;
    virtual inline ModuleType get_model_type() const = 0;
    virtual inline void set_coeffs(const double* new_poly_coeffs_x, const double* new_poly_coeffs_y) = 0;


protected:
    Point2d get_average_sample(const Point2d* ois_samples, uint32_t num_samples) const
    {
        Point2d sum(0.0, 0.0);

        for (uint32_t i = 0; i < num_samples; i++)
        {
            sum += ois_samples[i];
        }

        return sum / num_samples;
    }
};


class ShiftOIS : public OISOperator
{
protected:
    static const uint32_t   _num_coeffs = 10;
    float                   _poly_coeffs_x[_num_coeffs];       /** Coefficients of the x-axis conversation polynomial */
    float                   _poly_coeffs_y[_num_coeffs];       /** Coefficients of the y-axis conversation polynomial */

    static const uint32_t   _coeffs_domain_x = 1920;
    double                  _to_image_domain;                  /** Used to convert point from coefficients calibration domain to image domain */
    double                  _shift_to_ang;                     /** Used to convert shift in image domain to angle */

    /**
    *  Convert OIS reading code to image pixels
    *
    *  ois_hall_to_pixels(hall_x,hall_y) = p00 + p10*hall_x + p01*hall_y +
    *                                    + p20*hall_x^2 + p11*hall_x*hall_y + p02*hall_y^2 +
    *                                    + p30*hall_x^3 + p21*hall_x^2*hall_y + p12*hall_x*hall_y^2 + p03*hall_y^3
    **/
    #define ois_hall_to_pixels(hall_x, hall_y, poly_coeffs)        \
                (                                                          \
                    ( poly_coeffs[0]                                  ) +  \
                    ( poly_coeffs[1] * (hall_x)                       ) +  \
                    ( poly_coeffs[2] * (hall_y)                       ) +  \
                    ( poly_coeffs[3] * (hall_x) * (hall_x)            ) +  \
                    ( poly_coeffs[4] * (hall_x) * (hall_y)            ) +  \
                    ( poly_coeffs[5] * (hall_y) * (hall_y)            ) +  \
                    ( poly_coeffs[6] * (hall_x) * (hall_x) * (hall_x) ) +  \
                    ( poly_coeffs[7] * (hall_x) * (hall_x) * (hall_y) ) +  \
                    ( poly_coeffs[8] * (hall_x) * (hall_y) * (hall_y) ) +  \
                    ( poly_coeffs[9] * (hall_y) * (hall_y) * (hall_y) )    \
                )

public:
    ShiftOIS(const is_init_data_sensor&  sensor)
    :
        OISOperator(sensor)
    {
        IS_ASSERT((ModuleType::LENS_SHIFT == sensor.is_chromatix_info.ois.module_type) ||
                  (ModuleType::GIMBAL_PRISM_Y_LENS_SHIFT_X == sensor.is_chromatix_info.ois.module_type));

        IS_COMPILE_TIME_ASSERT(sizeof(_poly_coeffs_x) == sizeof(sensor.is_chromatix_info.ois.data_conversion_x));
        IS_COMPILE_TIME_ASSERT(sizeof(_poly_coeffs_y) == sizeof(sensor.is_chromatix_info.ois.data_conversion_y));

        IS_COMPILE_TIME_ASSERT(ARR_SIZE(_poly_coeffs_x) == ARR_SIZE(sensor.is_chromatix_info.ois.data_conversion_x));
        IS_COMPILE_TIME_ASSERT(ARR_SIZE(_poly_coeffs_y) == ARR_SIZE(sensor.is_chromatix_info.ois.data_conversion_y));

        memcpy(_poly_coeffs_x, sensor.is_chromatix_info.ois.data_conversion_x, sizeof(_poly_coeffs_x));
        memcpy(_poly_coeffs_y, sensor.is_chromatix_info.ois.data_conversion_y, sizeof(_poly_coeffs_y));
    }

    /* Update model with current camera and zoom */
    void update(const Camera& camera, const ZoomWindows& zoom)
    {
        // TODO (rgaizman): possible bug ?? need to tune for non sensor crop horizontal and expand according to sensor crop and ife crop horizontal
        _to_image_domain = ((double)camera.im_w / _coeffs_domain_x) / zoom.input_scale;
        _shift_to_ang = zoom.input.scale_x / camera.fx;
    }

    /* Requires update */
    Ang get_average_angle_o2i(const Point2d* ois_samples, uint32_t num_samples) const
    {
        const Point2d avg_ois = get_average_sample(ois_samples, num_samples);
        const Point2d avg_shift = to_shift_in_pixels(avg_ois);
        return Ang(-avg_shift(1) * _shift_to_ang, avg_shift(0) * _shift_to_ang, 0.0);
    }

    /* Requires update */
    Hom get_projectiv_o2i(Point2d ois) const
    {
        const Point2d shift = to_shift_in_pixels(ois);
        const Hom proj(1, 0, -shift.x,
                       0, 1, -shift.y,
                       0, 0, 1);
        return proj;
    }

    /* Requires update */
    inline Point2d apply_on_point_o2i(Point2d point, Point2d ois) const
    {
        return point - to_shift_in_pixels(ois);
    }

    /* Requires update */
    inline Point2d apply_on_point_i2o(Point2d point, Point2d ois) const
    {
        return point + to_shift_in_pixels(ois);
    }

    virtual inline ModuleType get_model_type() const
    {
        return ModuleType::LENS_SHIFT;
    }

    inline float get_coeff_x(uint32_t idx) const
    {
        IS_ASSERT(idx < _num_coeffs);
        return _poly_coeffs_x[idx];
    }

    inline float get_coeff_y(uint32_t idx) const
    {
        IS_ASSERT(idx < _num_coeffs);
        return _poly_coeffs_y[idx];
    }

    virtual inline void set_coeffs(const double* new_poly_coeffs_x, const double* new_poly_coeffs_y) override
    {
        for (uint32_t i = 0; i < _num_coeffs; i++)
        {
            _poly_coeffs_x[i] = (float)new_poly_coeffs_x[i];
            _poly_coeffs_y[i] = (float)new_poly_coeffs_y[i];
        }
    }

private:
    virtual Point2d to_shift_in_pixels(Point2d ois) const
    {
        const Point2d in(ois + _raw_offset);
        Point2d out;
        out.x = ois_hall_to_pixels(in.x, in.y, _poly_coeffs_x) * _to_image_domain;
        out.y = ois_hall_to_pixels(in.x, in.y, _poly_coeffs_y) * _to_image_domain;
        return  out;
    }
};

// This Operator deals onlky with x axis shift part
class GimbalPrismYLensShiftXOIS : public ShiftOIS
{
public:
    GimbalPrismYLensShiftXOIS(const is_init_data_sensor&  sensor)
    :
        ShiftOIS(sensor)
    {
        IS_ASSERT(ModuleType::GIMBAL_PRISM_Y_LENS_SHIFT_X == sensor.is_chromatix_info.ois.module_type);

        IS_COMPILE_TIME_ASSERT(sizeof(_poly_coeffs_x) == sizeof(sensor.is_chromatix_info.ois.data_conversion_x));
        IS_COMPILE_TIME_ASSERT(sizeof(_poly_coeffs_y) == sizeof(sensor.is_chromatix_info.ois.data_conversion_y));

        IS_COMPILE_TIME_ASSERT(ARR_SIZE(_poly_coeffs_x) == ARR_SIZE(sensor.is_chromatix_info.ois.data_conversion_x));
        IS_COMPILE_TIME_ASSERT(ARR_SIZE(_poly_coeffs_y) == ARR_SIZE(sensor.is_chromatix_info.ois.data_conversion_y));

        // should be done in ShiftOIS() constructor
        //memcpy(_poly_coeffs_x, sensor.is_chromatix_info.ois.data_conversion_x, sizeof(_poly_coeffs_x));

        for (uint32_t i = 0; i < _num_coeffs; i++)
        {
            _poly_coeffs_y[i] = 0;
        }
    }

    inline void set_coeffs(const double* new_poly_coeffs_x, const double* new_poly_coeffs_y)
    {
        (void)new_poly_coeffs_y;
        for (uint32_t i = 0; i < _num_coeffs; i++)
        {
            _poly_coeffs_x[i] = (float)new_poly_coeffs_x[i];
            _poly_coeffs_y[i] = 0;
        }
    }

    inline ModuleType get_model_type() const override
    {
        return ModuleType::GIMBAL_PRISM_Y_LENS_SHIFT_X;
    }

private:
    Point2d to_shift_in_pixels(Point2d ois) const override
    {
        const Point2d in(ois + _raw_offset);
        Point2d out;
        out.x = -ois_hall_to_pixels(in.x, 0, _poly_coeffs_x) * _to_image_domain;
        out.y = 0;
        return out;
    }
};

class GeneralOIS : public OISOperator
{
private:
    static const uint32_t   _num_poly_coeffs = 6;

public:
    static const uint32_t   _num_transform_coeffs = _num_poly_coeffs * 8;

private:
    double                  _coeffs_o2i[_num_transform_coeffs];
    double                  _coeffs_i2o[_num_transform_coeffs];
    uint32_t                _transform_defined_on_x;
    uint32_t                _transform_defined_on_y;

    Hom                     _camera;
    double                  _image_domain_x;
    double                  _image_domain_y;
    double                  _to_coeffs_domain_x;        /** Used to convert point from image domain to coefficients calibration domain */
    double                  _to_coeffs_domain_y;        /** Used to convert point from image domain to coefficients calibration domain */
    double                  _to_image_domain_x;         /** Used to convert point from coefficients calibration domain to image domain */
    double                  _to_image_domain_y;         /** Used to convert point from coefficients calibration domain to image domain */

public:
    GeneralOIS(const is_init_data_sensor&  sensor)
    :
        OISOperator(sensor)
    {
        IS_ASSERT(ModuleType::GENERAL_MODEL == sensor.is_chromatix_info.ois.module_type);

        uint32_t input_i = 0;
        for (uint32_t i = 0; i < _num_transform_coeffs; i++, input_i++)
        {
            _coeffs_o2i[i] = ois__general_model_parameters(sensor.is_chromatix_info, input_i);
        }
        for (uint32_t i = 0; i < _num_transform_coeffs; i++, input_i++)
        {
            _coeffs_i2o[i] = ois__general_model_parameters(sensor.is_chromatix_info, input_i);
        }
        _transform_defined_on_x = sensor.is_chromatix_info.ois__general_model_transform_defined_on_x;
        _transform_defined_on_y = sensor.is_chromatix_info.ois__general_model_transform_defined_on_y;
    }

    /* Update model with current camera and zoom */
    void update(const Camera& camera, const ZoomWindows& zoom)
    {
        // Asssumtion: zoom before EIS is centered
        _image_domain_x = (double)camera.im_w / zoom.input_scale;
        _image_domain_y = (double)camera.im_h / zoom.input_scale;
        _to_coeffs_domain_x = (double)_transform_defined_on_x / _image_domain_x;
        _to_coeffs_domain_y = (double)_transform_defined_on_y / _image_domain_y;
        _to_image_domain_x = 1.0 / _to_coeffs_domain_x;
        _to_image_domain_y = 1.0 / _to_coeffs_domain_y;
        _camera = camera.to_mat(); // TODO (asimchi): should consider zoom as well
    }

    /* Requires update */
    Ang get_average_angle_o2i(const Point2d* ois_samples, uint32_t num_samples) const
    {
        const Point2d avg_ois = get_average_sample(ois_samples, num_samples);
        const Hom proj = get_projectiv_o2i(avg_ois);
        const Ang avg_angle = perspective2angle(proj); // Inaccurate when general model projectiv are not perspective
        return avg_angle;
    }

    /* Requires update */
    Hom get_projectiv_o2i(Point2d ois) const
    {
        const Hom proj_coeffs_domain = get_projectiv(ois, _coeffs_o2i);
        const Hom proj = proj_coeffs_domain.ScaleMatrixCoordinates(
            (float)_transform_defined_on_x,
            (float)_transform_defined_on_y,
            (float)_image_domain_x,
            (float)_image_domain_y);
        return proj;
    }

    /* Requires update */
    inline Point2d apply_on_point_o2i(Point2d point, Point2d ois) const
    {
        return apply_on_point(point, ois, _coeffs_o2i);
    }

    /* Requires update */
    inline Point2d apply_on_point_i2o(Point2d point, Point2d ois) const
    {
        return apply_on_point(point, ois, _coeffs_i2o);
    }

    inline ModuleType get_model_type() const
    {
        return ModuleType::GENERAL_MODEL;
    }

    inline void set_coeffs(const double* new_poly_coeffs_x, const double* new_poly_coeffs_y)
    {
        (void)new_poly_coeffs_x;
        (void)new_poly_coeffs_y;
        IS_ASSERT(0);
    }

private:
    Point2d apply_on_point(Point2d point, Point2d ois, const double* coeffs) const
    {
        const Point2d point_coeffs_domain(point.x*_to_coeffs_domain_x, point.y*_to_coeffs_domain_y);
        const Point2d out_coeffs_domain = get_projectiv(ois, coeffs)*point_coeffs_domain;
        return Point2d(out_coeffs_domain.x*_to_image_domain_x, out_coeffs_domain.y*_to_image_domain_y);
    }

    Hom get_projectiv(Point2d ois, const double* coeffs) const
    {
        const Point2d in(ois + _raw_offset);

        const double xx = in.x*in.x;
        const double xy = in.x*in.y;
        const double yy = in.y*in.y;

        const double dim_1 = apply_poly(coeffs + _num_poly_coeffs * 0, in.x, in.y, xx, xy, yy);
        const double dim_2 = apply_poly(coeffs + _num_poly_coeffs * 1, in.x, in.y, xx, xy, yy);
        const double dim_3 = apply_poly(coeffs + _num_poly_coeffs * 2, in.x, in.y, xx, xy, yy);
        const double dim_4 = apply_poly(coeffs + _num_poly_coeffs * 3, in.x, in.y, xx, xy, yy);
        const double dim_5 = apply_poly(coeffs + _num_poly_coeffs * 4, in.x, in.y, xx, xy, yy);
        const double dim_6 = apply_poly(coeffs + _num_poly_coeffs * 5, in.x, in.y, xx, xy, yy);
        const double dim_7 = apply_poly(coeffs + _num_poly_coeffs * 6, in.x, in.y, xx, xy, yy);
        const double dim_8 = apply_poly(coeffs + _num_poly_coeffs * 7, in.x, in.y, xx, xy, yy);
        const double dim_9 = 1;

        return Hom(dim_1, dim_2, dim_3, dim_4, dim_5, dim_6, dim_7, dim_8, dim_9);
    }

    void scale_coefficients(uint32_t new_transform_defined_on_x, uint32_t new_transform_defined_on_y)
    {
        const double Sx = (float)new_transform_defined_on_x / _transform_defined_on_x;
        const double Sy = (float)new_transform_defined_on_y / _transform_defined_on_y;
        const double Ix = 1 / Sx;
        const double Iy = 1 / Sy;

        scale_poly(_coeffs_o2i + _num_poly_coeffs * 1, Iy * Sx);
        scale_poly(_coeffs_o2i + _num_poly_coeffs * 2, Sx);
        scale_poly(_coeffs_o2i + _num_poly_coeffs * 3, Ix * Sy);
        scale_poly(_coeffs_o2i + _num_poly_coeffs * 5, Sy);
        scale_poly(_coeffs_o2i + _num_poly_coeffs * 6, Ix);
        scale_poly(_coeffs_o2i + _num_poly_coeffs * 7, Iy);

        scale_poly(_coeffs_i2o + _num_poly_coeffs * 1, Iy * Sx);
        scale_poly(_coeffs_i2o + _num_poly_coeffs * 2, Sx);
        scale_poly(_coeffs_i2o + _num_poly_coeffs * 3, Ix * Sy);
        scale_poly(_coeffs_i2o + _num_poly_coeffs * 5, Sy);
        scale_poly(_coeffs_i2o + _num_poly_coeffs * 6, Ix);
        scale_poly(_coeffs_i2o + _num_poly_coeffs * 7, Iy);

        _transform_defined_on_x = new_transform_defined_on_x;
        _transform_defined_on_y = new_transform_defined_on_y;
    }

    inline static void scale_poly(double* p, double scale)
    {
        for (uint32_t i = 0; i < _num_poly_coeffs; i++)
        {
            p[i] = scale* p[i];
        }
    }

    inline static double apply_poly(const double* p, double x, double y, double xx, double xy, double yy)
    {
        return p[0] + p[1] * x + p[2] * y + p[3] * xx + p[4] * xy + p[5] * yy;
    }

    static Ang rot2angle(const Hom& rot)
    {
        //XYZ
        //const double s1 = -rot(0, 2);
        //const double c0c1 = rot(2, 2);
        //const double s0c1 = rot(1, 2);
        //const double s2c1 = rot(0, 1);
        //const double c2c1 = rot(0, 0);
        //const double s0p2 = -rot(1, 0);
        //const double c0p2 = rot(1, 1);

        //const double  c1 = sqrt(c0c1*c0c1 + s0c1*s0c1);

        //ang(0) = atan2(s0c1 / c1, c0c1 / c1);
        //ang(1) = atan2(s1, c1);
        //ang(2) = atan2(s2c1 / c1, c2c1 / c1);

        //TODO: optimize
        Ang ang;
        ang(0) = atan2(-rot(1, 2), rot(2, 2));
        ang(1) = asin(rot(0, 2));
        ang(2) = atan2(-rot(0, 1), rot(0, 0));

        return ang;
    }

    Ang perspective2angle(const Hom& perspective) const
    {
        Hom rot = _camera.inv()*perspective*_camera;

        rot = rot / rot(2, 2); //TODO: verify

        return rot2angle(rot);
    }
};

