//
//  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 uint32_t nPts    = (uint32_t) ((lsqdata_t*)p)->nPts;
    const uint32_t nCoords = (uint32_t) ((lsqdata_t*)p)->nCoords;
    const uint32_t nGroups = (uint32_t) ((lsqdata_t*)p)->nGroups;
    const uint32_t nLines  = (uint32_t) ((lsqdata_t*)p)->nLines;
    const uint32_t nMaxLinesPerGroup = (uint32_t) ((lsqdata_t*)p)->nMaxLinesPerGroup;
    
    const matrix_t* pDisPts      = ((lsqdata_t*)p)->pDisPts;
    const vector_t* pWeights     = ((lsqdata_t*)p)->pWeights;
    const matrix_t* pLineSizeMat = ((lsqdata_t*)p)->pLineSizeMat;
    
    vector_t *pAlphas = (vector_t*)malloc(sizeof(vector_t)*nGroups);
    vector_t *pBetas  = (vector_t*)malloc(sizeof(vector_t)*nGroups);
    vector_t *pGammas = (vector_t*)malloc(sizeof(vector_t)*nLines);
    
    assert( nPts == m );
    
    /*apply distortion model to correct image coordinates*/
    matrix_t* pDisCorrPts = (matrix_t*)malloc(sizeof(matrix_t)*nPts*nCoords);
    
    /*compute the corrected image coordinates (see Lens Distortion Correction Model)*/
    __std_distortion_func__(const_cast<matrix_t *>(pDisPts), pDisCorrPts, (coef_t*)x, nPts);
    
    /*compute best fit regression lines based on minimizing perpendicular offsets (constraint on group lines being parallel)*/
    harp_best_fit_lines(pAlphas, pBetas, pGammas, pDisCorrPts, pLineSizeMat, nPts, nGroups, nMaxLinesPerGroup);
    
    /*compute residual error between 'corrected' coordinate and regression line*/
    compute_residual(fvec, pDisCorrPts, pLineSizeMat, pAlphas, pBetas, pGammas, pWeights, nPts, nGroups, nLines, nMaxLinesPerGroup);

    /*clean up*/
    free( pAlphas );
    free( pBetas );
    free( pGammas );
    free( pDisCorrPts );
    
    return 0;
}
