//
//  lens_correction.cpp
//  lens_correction
//
//  Created by Mehdi Stapleton on 2015-06-23.
//  Copyright (c) 2015 Mehdi Stapleton. All rights reserved.
//

#include "lens_correction.h"
#include "interpolation.h"
#include "plumb_line.h"

//---------------------------------------------------------------
//                  IMAGE CORRECTION FUNCTIONS
//---------------------------------------------------------------

/* Apply correction model to an input 'distorted' image
 
 >blind correction: computes distorted coordinates given uniform grid of nominal correct coordinates,
 and uses interpolation for sub-pixel accuracy.
 
 */

void blind_image_correction(const matrix_t* pDisImg, matrix_t* const pCorrImg, const coef_t* pCoefs, const vector_t* pScale, const vector_t* pOffset, uint32_t nRows, uint32_t nCols, uint32_t nComps)
{
    /* construct abscissas and ordinate  (x,y) arrays
       (x,y) = normalized uniform grid
     */
    matrix_t* pX = (matrix_t*)malloc(sizeof(matrix_t)*nCols);
    matrix_t* pY = (matrix_t*)malloc(sizeof(matrix_t)*nRows);
    
    alglib::real_1d_array abscissas, ordinate;
    
    for(index_t c=0; c<nCols; ++c)
    {
        pX[c] = ((matrix_t)c)/((vector_t)pScale[0]) - pOffset[0];
        /* provide extra arguments for normalization
        //pX[c] = ((matrix_t)c)/((matrix_t)nCols) - 0.5;
         */
    }
    
    for(index_t r=0; r<nRows; ++r)
    {
        pY[r] = ((matrix_t)r)/((vector_t)pScale[1]) - pOffset[1];
        /* provide extra arguments for normalization
        //pY[r] = ((matrix_t)r)/((matrix_t)nRows) - 0.5;
         */
    }
    
    abscissas.setcontent(nCols, pX);
    ordinate.setcontent(nRows, pY);
    
    
    /* construct vector-valued function array
       NOTE: array is row-major format for compatibility 
             with spline interpolant constructor
     */
    matrix_t* pF = (matrix_t*)malloc(sizeof(matrix_t)*nRows*nCols*nComps);
    
    alglib::real_1d_array func;
    
    for(index_t r=0; r<nRows; ++r)
    {
        for(index_t c=0; c<nCols; ++c)
        {
            for(index_t i=0; i<nComps; ++i)
            {
                pF[nComps*(c + nCols*r) + i] = pDisImg[r + nRows*c + (nRows*nCols)*i];
            }
        }
    }
    
    func.setcontent(nRows*nCols*nComps, pF);
    
    
    /* build spline interpolant*/
    alglib::spline2dinterpolant bicubicInterp;
    
    alglib::spline2dbuildbicubicv(abscissas, nCols, ordinate, nRows, func, nComps, bicubicInterp);
    
    
    /* compute distorted coordinates and construct output image */
    matrix_t* pDisPt  = (matrix_t*)malloc(sizeof(matrix_t)*2);
    matrix_t* pCorrPt = (matrix_t*)malloc(sizeof(matrix_t)*2);
    
    alglib::real_1d_array disFunc;
    
    for(index_t r=0; r<nRows; ++r)
    {
        for(index_t c=0; c<nCols; ++c)
        {
            pCorrPt[0] = pX[c];
            pCorrPt[1] = pY[r];
            
            __blind_distortion_func__(pDisPt, pCorrPt, pCoefs, 1);
            
            alglib::spline2dcalcv(bicubicInterp, pDisPt[0], pDisPt[1], disFunc);
            
            for(index_t i=0; i<nComps; ++i)
            {
                pCorrImg[r + nRows*c + (nRows*nCols)*i] = disFunc(i);
            }
            
        }
    }
    
    
    /*clean-up*/
    free (pX);
    free (pY);
    free (pF);
    
    free (pDisPt);
    free (pCorrPt);
    
}






