//
//  plumb_line.h
//  plumb_line
//
//  Created by Mehdi Stapleton on 2015-05-18.
//
//

#ifndef __plumb_line__
#define __plumb_line__

//Standard libraries
#include <assert.h>
#include <cstdio>
#include <string>

#include <vector>
#include <cmath>

#include "plumb_line_types.h"


//---------------------------------------------------------------
//             GENERAL PREPROCESSOR DEFINITIONS
//---------------------------------------------------------------

#define __std_distortion_func__  standard_correction_model
#define __std_ncoefs__ STD_NCOEFS

#define __blind_distortion_func__ blind_std_correction_model
#define __fixed_point_operator__  blind_std_fixed_point_operator
#define __blind_ncoefs__ STD_NCOEFS


//---------------------------------------------------------------
//                      GLOBAL CONSTANTS
//---------------------------------------------------------------

#ifdef XCODE_COMPILATION
static const std::string PROJECT_DIR_PATH = "../common/";
#else
static const std::string PROJECT_DIR_PATH = "/Users/mehdistapleton/Documents/MATLAB/MASC/Camera Calibration Tools/Calibration Harp/standard plumb-line method/common/";
#endif

static const uint32_t nLensDisCtrParams = 2;


//---------------------------------------------------------------
//                  LENS DISTORTION FUNCTIONS
//---------------------------------------------------------------

/* Standard Polynomial Lens Distortion Correction Model:
 
 Function inputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 (double-precision) matrix
 
 2) pCorrPts    Desc: Current estimate of corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 3) pCoefsIn    Desc: Lens distortion model parameters
                Type: Qx1 vector
 
 4) nDataPts    Desc: Number of image coordinates (data points)
                Type: Scalar (>0)
 
 Function outputs:
 1) pCorrPts    Desc: Corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 coefs = [k1, k2, p1, p2, q1, q2, cx, cy]
 
 x_u = x_d*(k1*r^2 + k2*r^4) + q1*(x_d^2 + y_d^2) + x_d*(p1*x_d + p2*y_d) + x_d
 y_u = y_d*(k1*r^2 + k2*r^4) + q2*(x_d^2 + y_d^2) + y_d*(p1*x_d + p2*y_d) + y_d
 
 r^2 = (x_d-cx)^2 + (y_d - cy)^2
 
 >see: Wang, Jianhua, et al. "A new calibration model of camera lens distortion."
       Pattern Recognition 41.2 (2008): 607-615.
 
 */

void standard_correction_model(matrix_t* const pDisPts, matrix_t* const pCorrPts, const coef_t* pCoefsIn, uint32_t nDataPts);


/* Standard Field of View (FOV) Lens Distortion Correction Model
 
 Function inputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 (double-precision) matrix
 
 2) pCorrPts    Desc: Current estimate of corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 3) pCoefsIn    Desc: Lens distortion model parameters
                Type: Qx1 vector
 
 4) nDataPts    Desc: Number of image coordinates (data points)
                Type: Scalar (>0)
 
 Function outputs:
 1) pCorrPts     Desc: Corrected image coordinates
 Type: Nx2 matrix
 
 coefs = [w, k2, k3, cx, cy]
 
 R(r_u)      = (1/w)arctan( 2*r_u*tan(w/2) )
 R^{-1}(r_d) = tan(r_d*w)/( 2*tan(w/2) )
 
 r_u^2 = (x_u - cx)^2 + (y_u - cy)^2
 r_d^2 = (x_d - cx)^2 + (y_d - cy)^2
 
 x_u = ( R^{-1}(r_d) / r_d )*x_d + ( k2*r_d^2 + k3*r_d^4 )*x_d
 y_u = ( R^{-1}(r_d) / r_d )*y_d + ( k2*r_d^2 + k3*r_d^4 )*y_d
 
 >see: Devernay, Frederic, and Olivier Faugeras.
 "Straight lines have to be straight." Machine vision and
 applications 13.1 (2001): 14-24.
 
 */

void standard_fov_correction_model(matrix_t* const pDisPts, matrix_t* const pCorrPts, const coef_t* pCoefsIn, uint32_t nDataPts);


/* Standard Radial Model (RAD) Lens Distortion Correction Model
 
 Function inputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 (double-precision) matrix
 
 2) pCorrPts    Desc: Current estimate of corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 3) pCoefsIn    Desc: Lens distortion model parameters
                Type: Qx1 vector
 
 4) nDataPts    Desc: Number of image coordinates (data points)
                Type: Scalar (>0)
 
 Function outputs:
 1) pCorrPts     Desc: Corrected image coordinates
                 Type: Nx2 matrix
 
 coefs = [k0, k1, k2, k3, k4, cx, cy]
 
 (x_u - cx) = L(r_d)*(x_d - cx) 
 (y_u - cy) = L(r_d)*(y_d - cy)
 
 r_d^2 = (x_d - cx)^2 + (y_d - cy)^2
 
 L(r_d) = k0 + k1*r_d + k2*r_d^2 + k3*r_d^3
 
 x_u = L(r_d)*(x_d - cx) + cx
 y_u = L(r_d)*(y_d - cy) + cy
 
 >see: Alvarez, Luis, Luis Gomez, and J. Rafael Sendra. "Algebraic lens distortion
       model estimation." Image Processing On Line 1 (2010).
 
 */

void standard_rad_correction_model(matrix_t* const pDisPts, matrix_t* const pCorrPts, const coef_t* pCoefsIn, uint32_t nDataPts);


/* Blind Radial Model (RAD) Lens Distortion Correction Model
 
 Function inputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 (double-precision) matrix
 
 2) pCorrPts    Desc: Current estimate of corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 3) pCoefsIn    Desc: Lens distortion model parameters
                Type: Qx1 vector
 
 4) nDataPts    Desc: Number of image coordinates (data points)
                Type: Scalar (>0)
 
 Function outputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 matrix
 
 coefs = [k0, k1, k2, k3, k4, cx, cy]
 
 standard "inverse-transform" model proposed in paper:
 (x_u - cx) = L(r_d)*(x_d - cx)
 (y_u - cy) = L(r_d)*(y_d - cy)
 
 blind "forward-transform model"
 (x_d - cx) = L(r_u)*(x_u - cx)
 (y_d - cy) = L(r_u)*(y_u - cy)
 
 r_u^2 = (x_u- cx)^2 + (y_u - cy)^2
 
 L(r_u) = k0 + k1*r_u + k2*r_u^2 + k3*r_u^3
 
 x_d = L(r_u)*(x_u - cx) + cx
 y_d = L(r_u)*(y_u - cy) + cy
 
 >see: Alvarez, Luis, Luis Gomez, and J. Rafael Sendra. "Algebraic lens distortion
 model estimation." Image Processing On Line 1 (2010).
 
 */

void blind_rad_correction_model(matrix_t* const pDisPts, matrix_t* const pCorrPts, const coef_t* pCoefsIn, uint32_t nDataPts);

void blind_rad_fixed_point_operator(matrix_t* const pDisPts, matrix_t* const pCorrPts, matrix_t* const pNextPts, const coef_t* pCoefsIn, uint32_t nDataPts);


/* Blind Field of View (FOV) distortion model
 
 Function inputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 (double-precision) matrix
 
 2) pCorrPts    Desc: Current estimate of corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 3) pCoefsIn    Desc: Lens distortion model parameters
                Type: Qx1 vector
 
 4) nDataPts    Desc: Number of image coordinates (data points)
                Type: Scalar (>0)
 
 Function outputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 matrix
 
 coefs = [w, k2, k3, cx, cy]
 
 R(r_u)      = (1/w)arctan( 2*r_u*tan(w/2) )
 R^{-1}(r_d) = tan(r_d*w)/( 2*tan(w/2) )

 r_u^2 = (x_u - cx)^2 + (y_u - cy)^2
 r_d^2 = (x_d - cx)^2 + (y_d - cy)^2
 
 x_d = ( R(r_u) / r_u )*x_u + ( k2*r_u^2 + k3*r_u^4 )*x_u
 y_d = ( R(r_u) / r_u )*y_u + ( k2*r_u^2 + k3*r_u^4 )*y_u
 
 >see: Devernay, Frederic, and Olivier Faugeras. 
       "Straight lines have to be straight." Machine vision and
       applications 13.1 (2001): 14-24.
 
 */

void blind_fov_correction_model(matrix_t* const pDisPts, matrix_t* const pCorrPts, const coef_t* pCoefsIn, uint32_t nDataPts);

void blind_fov_fixed_point_operator(matrix_t* const pDisPts, matrix_t* const pCorrPts, matrix_t* const pNextPts, const coef_t* pCoefsIn, uint32_t nDataPts);

/* Blind Polynomial Lens Distortion Correction Model:
 
 Note: reverse of standard correction model
 
 Function inputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 (double-precision) matrix
 
 2) pCorrPts    Desc: Current estimate of corrected/Undistorted image coordinates
                Type: Nx2 matrix
 
 3) pCoefsIn    Desc: Lens distortion model parameters
                Type: Qx1 vector
 
 4) nDataPts    Desc: Number of image coordinates (data points)
                Type: Scalar (>0)
 
 Function outputs:
 1) pDisPts     Desc: Distorted image coordinates
                Type: Nx2 matrix
 
 Given model parameters k = [k1,k2,p1,p2,q1,q2,cx,cy]' ,
 
 Combined distortion:
 1)x_d = x + rad_x(x,y,k) + tan_x(x,y,k) + thn_x(x,y,k)
 2)y_d = y + rad_y(x,y,k) + tan_y(x,y,k) + thn_y(x,y,k)
 
 Radial distortion:
 rad_x(x,y,k) = x*(k1*r^2 + k2*r^4) ; rad_y(x,y,k) = y*(k1*r^2 + k2*r^4)
 
 Tangential distortion:
 tan_x(x,y,k) = x*(p1*x + p2*y); tan_y(x,y,k) = y*(p1*x + p2*y)
 
 Thin-Prism distortion:
 thn_x(x,y,k) = q1*(x^2 + y^2); thn_y(x,y,k) = q2*(x^2 + y^2)
 
 r^2 = (x - cx)^2 + (y - cy)^2
 
 x_d = distorted ;  x = undistorted
 y_d = distorted ;  y = undistorted
 
*/

void blind_std_correction_model(matrix_t* const pDisPts, matrix_t* const pCorrPts, const coef_t* pCoefsIn, uint32_t nDataPts);

void blind_std_fixed_point_operator(matrix_t* const pDisPts, matrix_t* const pCorrPts, matrix_t* const pNextPts, const coef_t* pCoefsIn, uint32_t nDataPts);


//---------------------------------------------------------------
//                  LINE-FITTING FUNCTIONS
//---------------------------------------------------------------

/* Best-fit Regression Line using Perpendicular Offsets
 
 regression line:
 alpha*x + beta*y = gamma
 
 objective:
 {alpha, beta, gamma} = arg min sum{all pts i} |alpha*x_i + beta*y_i - gamma| ; alpha = sin(theta), beta = cos(theta)
 
 assumptions:
 for harp images, all lines have same orientation (alpha, beta)
 
 */

void harp_best_fit_lines(vector_t* const pAlphas, vector_t* const pBetas, vector_t* const pGammas, const matrix_t* ptsIn, const matrix_t* lineSizeMatIn, uint32_t nDataPts, uint32_t nGroups, uint32_t nMaxLinesPerGroup);

void closest_line_points(const matrix_t* pPts, matrix_t* const pClosestPts, const matrix_t* pLineSizeMat, vector_t* const pAlphas, vector_t* const pBetas, vector_t* const pGammas, uint32_t nGroups, uint32_t nMaxLinesPerGroup, uint32_t nPts);

void compute_residual(matrix_t* const resOut, const matrix_t* ptsIn, const matrix_t* lineSizeMatIn, const coef_t* pAlphas, const coef_t* pBetas, const coef_t* pGammas, const vector_t* weightsIn, uint32_t nDataPts, uint32_t nGroups, uint32_t nLines, uint32_t nMaxLinesPerGroup);


#endif /* defined(__plumb_line__) */
