//
//  lsqnonlin.cpp
//  blind_lens_calibration_c
//
//  Created by Mehdi Stapleton on 2015-07-03.
//
//

#include "lsqnonlin.h"
#include "plumb_line.h"

//---------------------------------------------------------------
//                    LEAST-SQUARES COST FUNCTIONS
//---------------------------------------------------------------

int lsq_line_cost(void *p, int m, int n, const real *x, real *fvec, /*real *fjac, int ldfjac,*/ int iflag)
{
    const int nPts    = ((lsqdata_t*)p)->nPts;
    const int nCoords = ((lsqdata_t*)p)->nCoords;
    
    const real* pDisCtr  = ((lsqdata_t*)p)->pDisCtr;
    const real* pDisPts  = ((lsqdata_t*)p)->pDisPts;
    const real* pCorrPts = ((lsqdata_t*)p)->pCorrPts;
    
    assert( nPts*nCoords == m );
    
    /*append fixed distortion center parameters*/
    real* coefs = (real*)malloc(sizeof(real)*__blind_ncoefs__);
    
    memcpy(coefs, x, n*sizeof(real));
    if(n < __blind_ncoefs__)
    {
        memcpy((real*)(coefs+n), pDisCtr, (int)(__blind_ncoefs__-n)*sizeof(real));
    }
    
    /*apply distortion model to correct image coordinates*/
    matrix_t* pDisCorrPts = (matrix_t*)malloc(sizeof(matrix_t)*nPts*nCoords);
    
    __blind_distortion_func__( (matrix_t*)pDisCorrPts, (matrix_t*)pCorrPts, (coef_t*)coefs, (uint32_t)nPts );
    
    /*compute residual between distorted correct image coordinates and measured distorted coordinates*/
    if (true)/*(iflag!=2)*/
    {
        for(index_t i =0; i< nPts; ++i)
        {
            for(index_t j=0; j<nCoords; ++j)
            {
                 fvec[i + j*nPts] = pDisPts[i + j*nPts] - pDisCorrPts[i + j*nPts];
            }
        }
    }
    
    free (coefs);
    free (pDisCorrPts);
    
    return 0;
}
