//
//  main.cpp
//  subpixel_lsd
//
//  Created by Mehdi Stapleton on 2015-05-18.
//  Copyright (c) 2015 Mehdi Stapleton. All rights reserved.
//

#include "subpixel_lsd.h"

//---------------------------------------------------------------
//                          DESCRIPTION
//---------------------------------------------------------------

/*Mex Function Inputs:
 1)Edge Magnitude (Gradient Norm) Image     Desc:L2-norm  (modulus) of gradient.
                                            Type:NxM matrix (double)
 2)Angle (Gradient) Image    Desc:Direction of gradient (col+ = +x, row+ = +y).
                             Type:NxM matrix (double)
 */

/*Mex Function Outputs:
 1)Line Labels  Desc:Each identified line segment has a unique label (#>1) assigned to 
                     respective pixels. Label 0 for pixel corresponds to no associated line.
                Type:NxM matrix (double)
 */

/*Background
 >searchs for strong line candidates in image based on gradient angle uniformity and
  minimizing NFA (number of false alarms)
 >see: Von Gioi, R. Grompone, et al. "LSD: a line segment detector." 
       Image Processing On Line 2.3 (2012): 5.
 */


#ifndef XCODE_COMPILATION

//-------------------------------------------------------------------------
//              MEX File Generator Function Entry-Point
//-------------------------------------------------------------------------

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    /* Macros for the ouput and input arguments */
#define EDGE_IMAGE_IN      prhs[0]
#define ANGLE_IMAGE_IN     prhs[1]
#define ANGLE_TOL_IN       prhs[2]
#define DENSITY_TOL_IN     prhs[3]
    
#define LINE_LABELS_OUT    plhs[0]
#define NLINES_OUT         plhs[1]
#define LINE_INFO_OUT      plhs[2]
    
    matrix_t *edgeImgIn;
    matrix_t *angleImgIn;
    double   *pAngleTolIn;
    double   *pDensityTolIn;
    
    matrix_t *lineLabelsOut;
    double   *lineInfoOut;
    
    Image<pixel_t> edgeImg;
    Image<pixel_t> angleImg;
    
    mwSize        nImgDims;
    const mwSize* imgDims;
    
    const mwSize  nLineParams = 7;
    const mwSize  nInfoDims = 2;
    mwSize        *infoDims;
    
    size_t        nRows,nCols;
    
    if(nrhs != 4) /* Check the number of input arguments */
    {
        LOG_ERROR("Wrong number of input arguments.");
    }
    else if(nlhs != 3) /* Check the number of output arguments */
    {
        LOG_ERROR("Too many output arguments.");
    }
    
    if(!IS_REAL_2D_FULL_DOUBLE(EDGE_IMAGE_IN) || !IS_REAL_2D_FULL_DOUBLE(ANGLE_IMAGE_IN))
    {
        LOG_ERROR("Input edge/angle images must be a real 2D full double arrays.");
    }
    
    if(!IS_REAL_SCALAR(ANGLE_TOL_IN) || !IS_REAL_SCALAR(DENSITY_TOL_IN))
    {
        LOG_ERROR("Input rectangle-density and angle tolerance must be real double scalars.");
    }
    
    nImgDims = mxGetNumberOfDimensions(EDGE_IMAGE_IN);
    imgDims  = mxGetDimensions(EDGE_IMAGE_IN);
    nRows    = imgDims[0];
    nCols    = imgDims[1];
    
    if( nRows <= 0 || nCols <= 0)
    {
        LOG_ERROR("Invalid number of rows or columns");
    }
    
    LOG_DEBUG("Received image with dimensions " << nRows << "x" << nCols);
    
    edgeImgIn     = mxGetPr(EDGE_IMAGE_IN);
    angleImgIn    = mxGetPr(ANGLE_IMAGE_IN);
    pAngleTolIn   = mxGetPr(ANGLE_TOL_IN);
    pDensityTolIn = mxGetPr(DENSITY_TOL_IN);
    
    LINE_LABELS_OUT = mxCreateNumericArray(nImgDims, imgDims, mxDOUBLE_CLASS, mxREAL);
    
    lineLabelsOut = (matrix_t*)mxGetData(LINE_LABELS_OUT);
    
    edgeImg.width   = nCols;
    edgeImg.height  = nRows;
    angleImg.width  = nCols;
    angleImg.height = nRows;
    
    //(matlab column-major -> lsd row-major)
    for(index_t r=0; r<nRows; ++r)
    {
        for(index_t c=0; c<nCols; ++c)
        {
            edgeImg.pixels.push_back((pixel_t)edgeImgIn[r+nRows*c]);
            angleImg.pixels.push_back((pixel_t)angleImgIn[r+nRows*c]);
        }
    }
    
    //perform line-segment detection
    const double scale = 1;
    double* lineStats;
    int*    lsdLineLabels;
    int     nLines, nColsOut, nRowsOut;
    
    lsdLineLabels = (int*)calloc( (size_t)nCols*nRows, sizeof(int) );
    
    lineStats = lsd_mod_region( &nLines
                                ,edgeImg.pixels.data(), angleImg.pixels.data()
                                ,(int)nCols, (int)nRows
                                ,&lsdLineLabels, &nColsOut, &nRowsOut
                                ,*pAngleTolIn, *pDensityTolIn);
    
    infoDims = (mwSize*) malloc(sizeof(mwSize)*nInfoDims);
    infoDims[0] = (mwSize)nLines;
    infoDims[1] = nLineParams;
    LINE_INFO_OUT = mxCreateNumericArray(nInfoDims, infoDims, mxDOUBLE_CLASS, mxREAL);
    lineInfoOut   = (double*) mxGetData(LINE_INFO_OUT);
    
    NLINES_OUT = mxCreateDoubleScalar((double)nLines);
    
#ifdef DEBUG_MODE
    //display line segment information
    for(index_t i=0; i< nLines; ++i)
    {
        LOG_DEBUG("Line" << i << ": " << lineStats[0+i*7] << ","
                                      << lineStats[1+i*7] << ","
                                      << lineStats[2+i*7] << ","
                                      << lineStats[3+i*7] << ","
                                      << lineStats[4+i*7] << ","
                                      << lineStats[5+i*7] << ","
                                      << lineStats[6+i*7] );
    }
#endif
    
    //(lsd row-major -> matlab column-major)
    for(index_t r=0; r<(index_t)nLines; ++r)
    {
        for(index_t c=0; c<(index_t)nLineParams; ++c)
        {
            lineInfoOut[r + nLines*c] = (matrix_t)lineStats[c + nLineParams*r];
        }
    }
     
    //(lsd row-major -> matlab column-major)
    for(index_t r=0; r<nRowsOut; ++r)
    {
        for(index_t c=0; c<nColsOut; ++c)
        {
            lineLabelsOut[r + nRows*c] = (matrix_t)lsdLineLabels[c + nColsOut*r];
        }
    }
    
    free( (void*) lsdLineLabels);
    
}//end of mexFunction

#endif//XCODE_COMPILATION