#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>

#include "liblbp.h"
#include "flandmark_detector.h"

void flandmark_writeModel(const char* filename, FLANDMARK_Model* model)
{
    int * p_int = 0, tsize = -1, tmp_tsize = -1;
    uint8_t * p_uint8 = 0;
    uint32_t * p_uint32 = 0;

    FILE *fout;
    if ((fout = fopen(filename, "wb")) == NULL)
    {
        printf("Error opening file %s\n", filename);
        exit(1);
    }

    // write constants (size of matrices, etc)
    fprintf(fout, " %c ", model->data.options.M);
    fprintf(fout, " %d %d ", model->data.options.bw[0], model->data.options.bw[1]);
    fprintf(fout, " %d %d ", model->data.options.bw_margin[0], model->data.options.bw_margin[1]);
    fprintf(fout, " %d %d ", model->W_ROWS, model->W_COLS);
    fprintf(fout, " %d %d ", model->data.imSize[0], model->data.imSize[1]);
    for (int idx = 0; idx < model->data.options.M; ++idx)
    {
        fprintf(fout, " %d %d ", model->data.lbp[idx].WINS_ROWS, model->data.lbp[idx].WINS_COLS);
    }
    for (int idx = 0; idx < 3; ++idx)
    {
        fprintf(fout, " %d %d ", model->data.options.PSIG_ROWS[idx], model->data.options.PSIG_COLS[idx]);
        printf("model->data.options.PSIG_ROWS[%d] = %d; model->data.options.PSIG_COLS[%d] = %d; \n",
                idx, model->data.options.PSIG_ROWS[idx],
                idx, model->data.options.PSIG_COLS[idx] );
    }

    // write model->W
    printf("Writing model->W to file... ");
    if (fwrite(model->W, model->W_ROWS*sizeof(double), 1, fout) != 1)
    {
        fclose(fout);
        printf("Error writing file %s\n", filename);
        exit(1);
    }
    printf("done.\n");

    // write model->mapTable
    printf("Writing model->data.mapTable to file... ");
    if (fwrite(model->data.mapTable, model->data.options.M*4*sizeof(int), 1, fout) != 1)
    {
        fclose(fout);
        printf( "Error writing file %s\n", filename);
        exit(1);
    }
    printf( "done.\n");

    // write model->data.lbp ---------------------------------------------------
    printf( "Writing model->data.lbp to file... \n");
    for (int idx = 0; idx < model->data.options.M; ++idx)
    {
        printf( "lbp[%d]... ", idx);

        // lbp{idx}.winSize
        p_int = &(model->data.lbp[idx].winSize[0]);
        if (fwrite(p_int, 2*sizeof(int), 1, fout) != 1)
        {
            fclose(fout);
            printf( "Error writing file %s\n", filename);
            exit(1);
        }
        printf( " winSize... ");

        // lbp{idx}.hop
        p_uint8 = &(model->data.lbp[idx].hop);
        if (fwrite(p_uint8, sizeof(uint8_t), 1, fout) != 1)
        {
            fclose(fout);
            printf( "Error writing file %s\n", filename);
            exit(1);
        }
        printf( " hop... ");

        // lbp{idx}.wins
        p_uint32 = model->data.lbp[idx].wins;
        tsize = model->data.lbp[idx].WINS_ROWS*model->data.lbp[idx].WINS_COLS;
        if (fwrite(p_uint32, tsize*sizeof(uint32_t), 1, fout) != 1)
        {
            fclose(fout);
            printf( "Error writing file %s\n", filename);
            exit(1);
        }
        printf( " wins... done.\n");
    }

    // write model->data.options.S --------------------------------------------
    printf( "Writing model->data.options.S to file... ");
    p_int = &(model->data.options.S[0]);
    if (fwrite(p_int, 4*model->data.options.M*sizeof(int), 1, fout) != 1)
    {
        fclose(fout);
        printf( "Error writing file %s\n", filename);
        exit(1);
    }
    printf( "done.\n");

    // write model->data.options.PsiGS# -----------------------------------------
    FLANDMARK_PSIG * PsiGi = NULL;
    for (int psigs_idx = 0; psigs_idx < 3; ++psigs_idx)
    {
        printf("PsiGS for loop.\n");
        switch (psigs_idx)
        {
            case 0:
                printf( "Case 0 = PsiGS0 setting pointer...");
                PsiGi = (model->data.options.PsiGS0);
                printf( " done.\n");
                break;
            case 1:
                printf( "Case 0 = PsiGS1 setting pointer...");
                PsiGi = (model->data.options.PsiGS1);
                printf( " done.\n");
                break;
            case 2:
                printf( "Case 0 = PsiGS2 setting pointer...");
                PsiGi = (model->data.options.PsiGS2);
                printf( " done.\n");
                break;
        }
        
        printf("calculating tsize\n");
        
        tsize = model->data.options.PSIG_ROWS[psigs_idx]*model->data.options.PSIG_COLS[psigs_idx];
        
        printf("tsize = %d\n", tsize);
        
        for (int idx = 0; idx < tsize; ++idx)
        {
            // write ROWS and COLS size
            p_int = &PsiGi[idx].ROWS;
            if (fwrite(p_int, sizeof(int), 1, fout) != 1)
            {
                fclose(fout);
                printf( "Error writing file %s\n", filename);
                exit(1);
            }
            p_int = &PsiGi[idx].COLS;
            if (fwrite(p_int, sizeof(int), 1, fout) != 1)
            {
                fclose(fout);
                printf( "Error writing file %s\n", filename);
                exit(1);
            }
            // write disp
            tmp_tsize = PsiGi[idx].ROWS*PsiGi[idx].COLS;
            if (fwrite(PsiGi[idx].disp, tmp_tsize*sizeof(int), 1, fout) != 1)
            {
                fclose(fout);
                printf( "Error writing file %s\n", filename);
                exit(1);
            }
        }
    }

    fclose(fout);
}

FLANDMARK_Model * flandmark_init(const char* filename)
{
    int *p_int = 0, tsize = -1, tmp_tsize = -1;
    uint8_t *p_uint8 = 0;

    FILE *fin;
    if ((fin = fopen(filename, "rb")) == NULL)
    {
        printf("Error opening file %s\n", filename);
        return 0;
    }

    // allocate memory for FLANDMARK_Model
    FLANDMARK_Model * tst = (FLANDMARK_Model*)malloc(sizeof(FLANDMARK_Model));

    int fscan_ret = -1;

    fscan_ret = fscanf(fin, " %c ", &tst->data.options.M);
    fscan_ret = fscanf(fin, " %d %d ", &tst->data.options.bw[0], &tst->data.options.bw[1]);
    fscan_ret = fscanf(fin, " %d %d ", &tst->data.options.bw_margin[0], &tst->data.options.bw_margin[1]);
    fscan_ret = fscanf(fin, " %d %d ", &tst->W_ROWS, &tst->W_COLS);
    //fscan_ret = fscanf(fin, " %d %d ", &tst->data.Images_ROWS, &tst->data.Images_COLS);
    fscan_ret = fscanf(fin, " %d %d ", &tst->data.imSize[0], &tst->data.imSize[1]);

    int M = tst->data.options.M;
    
    tst->data.lbp = (FLANDMARK_LBP*)malloc(tst->data.options.M*sizeof(FLANDMARK_LBP));
    for (int idx = 0; idx < tst->data.options.M; ++idx)
    {
        fscan_ret = fscanf(fin, " %d %d ", &tst->data.lbp[idx].WINS_ROWS, &tst->data.lbp[idx].WINS_COLS);
    }
    for (int idx = 0; idx < 3; ++idx)
    {
        fscan_ret = fscanf(fin, " %d %d ", &tst->data.options.PSIG_ROWS[idx], &tst->data.options.PSIG_COLS[idx]);
    }
    
    // load model.W -----------------------------------------------------------
    tst->W = (double*)malloc(tst->W_ROWS * sizeof(double));
    if (fread(tst->W, tst->W_ROWS * sizeof(double), 1, fin) != 1)
    {
        printf( "Error reading file %s\n", filename);
        return 0;
        //exit(1);
    }

    // load model.data.mapTable -----------------------------------------------
    p_int = (int*)malloc(tst->data.options.M*4*sizeof(int));
    tst->data.mapTable = (int*)malloc(M*4*sizeof(int));
    if (fread(p_int, tst->data.options.M*4*sizeof(int), 1, fin) != 1)
    {
        printf( "Error reading file %s\n", filename);
        return 0;
        //exit(1);
    }
    for (int i = 0; i < tst->data.options.M*4; ++i)
    {
        tst->data.mapTable[i] = p_int[i];
    }
    free(p_int);

    // load model.data.lbp ---------------------------------------------------
    for (int idx = 0; idx < tst->data.options.M; ++idx)
    {
        // lbp{idx}.winSize
        p_int = (int*)malloc(2*sizeof(int));
        if (fread(p_int, 2*sizeof(int), 1, fin) != 1)
        {
            printf( "Error reading file %s\n", filename);
            return 0;
            //exit(1);
        }
        for (int i = 0; i < 2; ++i)
        {
            tst->data.lbp[idx].winSize[i] = p_int[i];
        }
        free(p_int);

        // lbp{idx}.hop
        p_uint8 = (uint8_t*)malloc(sizeof(uint8_t));
        if (fread(p_uint8, sizeof(uint8_t), 1, fin) != 1)
        {
            printf( "Error reading file %s\n", filename);
            return 0;
            //exit(1);
        }
        tst->data.lbp[idx].hop = p_uint8[0];
        free(p_uint8);

        // lbp{idx}.wins
        tsize = tst->data.lbp[idx].WINS_ROWS*tst->data.lbp[idx].WINS_COLS;
        tst->data.lbp[idx].wins = (uint32_t*)malloc(tsize * sizeof(uint32_t));
        if (fread(tst->data.lbp[idx].wins, tsize * sizeof(uint32_t), 1, fin) != 1)
        {
            printf( "Error reading file %s\n", filename);
            return 0;
            //exit(1);
        }
    }

    // load model.options.S --------------------------------------------------
    p_int = (int*)malloc(4*tst->data.options.M*sizeof(int));
    tst->data.options.S = (int*)malloc(4*M*sizeof(int));
    if (fread(p_int, 4*tst->data.options.M*sizeof(int), 1, fin) != 1)
    {
        printf( "Error reading file %s\n", filename);
        return 0;
        //exit(1);
    }
    for (int i = 0; i < 4*tst->data.options.M; ++i)
    {
        tst->data.options.S[i] = p_int[i];
    }
    free(p_int);

    // load model.options.PsiG -----------------------------------------------
    FLANDMARK_PSIG * PsiGi = NULL;
    for (int psigs_idx = 0; psigs_idx < 3; ++psigs_idx)
    {
        
        tsize = tst->data.options.PSIG_ROWS[psigs_idx]*tst->data.options.PSIG_COLS[psigs_idx];
        
        switch (psigs_idx)
        {
            case 0:
                tst->data.options.PsiGS0 = (FLANDMARK_PSIG*)malloc(tsize*sizeof(FLANDMARK_PSIG));
                PsiGi = tst->data.options.PsiGS0;
                break;
            case 1:
                tst->data.options.PsiGS1 = (FLANDMARK_PSIG*)malloc(tsize*sizeof(FLANDMARK_PSIG));
                PsiGi = tst->data.options.PsiGS1;
                break;
            case 2:
                tst->data.options.PsiGS2 = (FLANDMARK_PSIG*)malloc(tsize*sizeof(FLANDMARK_PSIG));
                PsiGi = tst->data.options.PsiGS2;
                break;
        }
        
        for (int idx = 0; idx < tsize; ++idx)
        {
            // disp ROWS
            p_int = (int*)malloc(sizeof(int));
            if (fread(p_int, sizeof(int), 1, fin) != 1)
            {
                printf( "Error reading file %s\n", filename);
                return 0;
                //exit(1);
            }
            PsiGi[idx].ROWS = p_int[0];
            free(p_int);
            // disp COLS
            p_int = (int*)malloc(sizeof(int));
            if (fread(p_int, sizeof(int), 1, fin) != 1)
            {
                printf( "Error reading file %s\n", filename);
                return 0;
                //exit(1);
            }
            PsiGi[idx].COLS = p_int[0];
            free(p_int);
            // disp
            tmp_tsize = PsiGi[idx].ROWS*PsiGi[idx].COLS;
            PsiGi[idx].disp = (int*)malloc(tmp_tsize*sizeof(int));
            if (fread(PsiGi[idx].disp, tmp_tsize*sizeof(int), 1, fin) != 1)
            {
                printf( "Error reading file %s\n", filename);
                return 0;
                //exit(1);
            }
        }
    }

    fclose(fin);

    // preallocate OpenCV Images
    tst->p_width = 640;
    tst->p_height = 480;
//    tst->p_width = 10;
//    tst->p_height = 10;
    tst->normalizedImageFrame = (uint8_t*)malloc(tst->data.options.bw[0]*tst->data.options.bw[1]*sizeof(uint8_t));
    tst->bb = (double*)malloc(4*sizeof(double));
    tst->sf = (float*)malloc(2*sizeof(float));
    tst->croppedImage = cvCreateImage(cvSize(tst->p_width, tst->p_height), IPL_DEPTH_8U, 1);
    tst->resizedImage = cvCreateImage(cvSize(tst->p_width, tst->p_height), IPL_DEPTH_8U, 1);

    return tst;
}

EError_T flandmark_checkModel(FLANDMARK_Model* model, FLANDMARK_Model* tst)
{
    bool flag = false;
    int tsize = -1, tmp_tsize = -1;

    // check model->data.options.M
    printf("Checking mode->data.options.M...");
    flag = true;
    if (model->data.options.M != tst->data.options.M)
    {
        flag = false;
        printf("\n: %d ; %d", model->data.options.M, tst->data.options.M);
    }
    flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
    if (!flag)
    {
        return ERROR_M;
    }
    
    int M = model->data.options.M;
    
    // chceck model->data.options.bw
    printf("Checking mode->data.options.bw...");
    flag = true;
    if (model->data.options.bw[0] != tst->data.options.bw[0] ||
            model->data.options.bw[1] != tst->data.options.bw[1])
    {
        flag = false;
        printf("\n: %d ; %d", model->data.options.bw[0], tst->data.options.bw[0]);
        printf("\n: %d ; %d", model->data.options.bw[1], tst->data.options.bw[1]);
    }
    flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
    if (!flag)
    {
        return ERROR_BW;
    }
    
    // chceck model->data.options.bw_margin
    printf("Checking mode->data.options.bw_margin...");
    flag = true;
    if (model->data.options.bw_margin[0] != tst->data.options.bw_margin[0] ||
            model->data.options.bw_margin[1] != tst->data.options.bw_margin[1])
    {
        flag = false;
        printf("\n: %d ; %d", model->data.options.bw_margin[0], tst->data.options.bw_margin[0]);
        printf("\n: %d ; %d", model->data.options.bw_margin[1], tst->data.options.bw_margin[1]);
    }
    flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
    if (!flag)
    {
        return ERROR_BW_MARGIN;
    }
    
    // check model->W
    printf( "Checking model->W... ");
    flag = true;
    for (int i = 0; i < tst->W_ROWS; ++i)
    {
        if (model->W[i] != tst->W[i])
        {
            flag = false;
            printf( "\n%d: %f ; %f", i, model->W[i], tst->W[i]);
            printf( "Error.");
        }
    }
    flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
    if (!flag)
    {
        return ERROR_W;
    }

    // check model->data.mapTable
    printf( "Checking model->data.mapTable... ");
    flag = true;
    for (int i = 0; i < M*4; ++i)
    {
        if (model->data.mapTable[i] != tst->data.mapTable[i])
        {
            flag = false;
            printf( "\n%d: %d ; %d", i, model->data.mapTable[i], tst->data.mapTable[i]);
            printf( "Error.");
        }
    }
    flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
    if (!flag)
    {
        return ERROR_DATA_MAPTABLE;
    }

    // load model->data.lbp ---------------------------------------------------
    for (int idx = 0; idx < model->data.options.M; ++idx)
    {
        flag = true;
        printf( "checking lbp[%d]... ", idx);

        for (int i = 0; i < 2; ++i)
        {
            if (tst->data.lbp[idx].winSize[i] != model->data.lbp[idx].winSize[i])
            {
                flag = false;
                printf( "\n%d: %d ; %d", i, model->data.lbp[idx].winSize[i], tst->data.lbp[idx].winSize[i]);
                printf( "Error.");
            }
        }

        // lbp{idx}.hop
        if (tst->data.lbp[idx].hop != model->data.lbp[idx].hop)
        {
            flag = false;
            printf( "\n %d ; %d", model->data.lbp[idx].hop, tst->data.lbp[idx].hop);
            printf( "Error.");
        }

        // lbp{idx}.wins
        tsize = tst->data.lbp[idx].WINS_ROWS*tst->data.lbp[idx].WINS_COLS;
        for (int i = 0; i < tsize; ++i)
        {
            if (model->data.lbp[idx].wins[i] != tst->data.lbp[idx].wins[i])
            {
                flag = false;
                printf( "\n%d: %d ; %d", i, model->data.lbp[idx].wins[i], tst->data.lbp[idx].wins[i]);
                printf( "Error.");
            }
        }
        flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
        if (!flag)
        {
            return ERROR_DATA_LBP;
        }
    }

    // check model->data.options.S
    printf( "Checking model->data.options.S... ");
    flag = true;
    for (int i = 0; i < 4*M; ++i)
    {
        if (model->data.options.S[i] != tst->data.options.S[i])
        {
            flag = false;
            printf( "\n%d: %d ; %d", i, model->data.options.S[i], tst->data.options.S[i]);
            printf("Error.");
        }
    }
    flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
    if (!flag)
    {
        return ERROR_DATA_OPTIONS_S;
    }

    // check model->data.options.PsiG
    FLANDMARK_PSIG *PsiGi = NULL, *PsiGiTst = NULL;
    for (int psig_idx = 0; psig_idx < 3; ++psig_idx)
    {
        switch(psig_idx)
        {
            case 0: 
                PsiGi = (model->data.options.PsiGS0);
                PsiGiTst = (tst->data.options.PsiGS0);
                break;
            case 1:
                PsiGi = (model->data.options.PsiGS1);
                PsiGiTst = (tst->data.options.PsiGS1);
                break;
            case 2:
                PsiGi = (model->data.options.PsiGS2);
                PsiGiTst = (tst->data.options.PsiGS2);
                break;
        }
        flag = true;
        printf( "Checking model->data.options.PsiGS%d\n", psig_idx);
        printf( "options.PSIG_ROWS[%d]; options.PSIG_COLS[%d]... ", psig_idx, psig_idx);
        if (model->data.options.PSIG_ROWS[psig_idx] != tst->data.options.PSIG_ROWS[psig_idx] ||
            model->data.options.PSIG_COLS[psig_idx] != tst->data.options.PSIG_COLS[psig_idx])
        {
            flag = false;
            printf("Error.");
        }
        flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
        if (!flag)
        {
            return ERROR_DATA_OPTIONS_PSIG;
        }
        // disp
        flag = true;
        tsize = tst->data.options.PSIG_ROWS[psig_idx]*tst->data.options.PSIG_COLS[psig_idx];
        printf( "options.PsiGS%d...", psig_idx);
        for (int idx = 0; idx < tsize; ++idx)
        {
            if (PsiGi[idx].ROWS != PsiGiTst[idx].ROWS ||
                PsiGi[idx].COLS != PsiGiTst[idx].COLS)
            {
                printf( "\nPsiGS[%d].ROWS\n", psig_idx, idx);
                flag = false;
                printf("Error.");
            }
            tmp_tsize = PsiGiTst[idx].ROWS*PsiGiTst[idx].COLS;
            for (int i = 0; i < tmp_tsize; ++i)
            {
                if (PsiGi[idx].disp[i] != PsiGiTst[idx].disp[i])
                {
                    flag = false;
                    printf( "\nPsiGS%d[%d] =  %d; %d\n", psig_idx, idx, PsiGi[idx].disp[i], PsiGiTst[idx].disp[i]);
                    printf( "Error.");
                }
            }
        }
        flag == true ? printf( "passed. \n") : printf( "NOT passed.\n");
        if (!flag)
        {
            return ERROR_DATA_OPTIONS_PSIG;
        }
    }
    return NO_ERR;
}

void flandmark_free(FLANDMARK_Model* model)
{
    FLANDMARK_PSIG *PsiGi = NULL;
    for (int psig_idx = 0; psig_idx < 3; ++psig_idx)
    {
        switch(psig_idx)
        {
            case 0:
                PsiGi = (model->data.options.PsiGS0);
                break;
            case 1:
                PsiGi = (model->data.options.PsiGS1);
                break;
            case 2:
                PsiGi = (model->data.options.PsiGS2);
                break;
        }
        
        int tsize = model->data.options.PSIG_ROWS[psig_idx] * model->data.options.PSIG_COLS[psig_idx];
        for (int i = 0; i < tsize; ++i)
        {
            free(PsiGi[i].disp);
        }
        free(PsiGi);
    }
    
    free(model->W);
    for (int i = 0; i < model->data.options.M; ++i)
    {
        free(model->data.lbp[i].wins);
    }
    free(model->data.lbp);
    free(model->data.options.S);
    free(model->data.mapTable);

    if (model->croppedImage)
        cvReleaseImage(&model->croppedImage);
    if (model->resizedImage)
        cvReleaseImage(&model->resizedImage);
    if (model->normalizedImageFrame)
        free(model->normalizedImageFrame);
    if (model->bb)
        free(model->bb);
    if (model->sf)
        free(model->sf);

    free(model);
}

void flandmark_getPsiMat(FLANDMARK_PSI* Psi, FLANDMARK_Model* model, int lbpidx)
{
    char * Features;
    uint8_t * Images = model->normalizedImageFrame;
    uint32_t im_H = (uint32_t)model->data.imSize[0]; uint32_t im_W = (uint32_t)model->data.imSize[1];
    uint32_t * Wins = model->data.lbp[lbpidx].wins;
    uint16_t win_H = (uint16_t)model->data.lbp[lbpidx].winSize[0]; uint16_t win_W = (uint16_t)model->data.lbp[lbpidx].winSize[1];
    uint16_t nPyramids = model->data.lbp[lbpidx].hop;
    uint32_t nDim = liblbp_pyr_get_dim(win_H, win_W, nPyramids); uint32_t nData = model->data.lbp[lbpidx].WINS_COLS;

    Features = (char*)calloc(nDim*nData, sizeof(char));
    if (Features == NULL)
    {
        printf( "Not enough memory for LBP features.\n");
        exit(1);
    }
    Psi->PSI_ROWS = nDim; Psi->PSI_COLS = nData;

    uint32_t cnt, cnt0, mirror, x, x1, y, y1, idx, *win;
    uint8_t *img_ptr;

    win = (uint32_t*)malloc(win_H*win_W*sizeof(uint32_t));
    if(win == NULL)
    {
        printf( "Not enough memory for cropped_window.\n");
        exit(1);
    }

    cnt = 0;
    for(uint32_t i = 0; i < nData; ++i)
    {
        idx = Wins[INDEX(0,i,4)]-1;
        x1  = Wins[INDEX(1,i,4)]-1;
        y1  = Wins[INDEX(2,i,4)]-1;
        mirror = Wins[INDEX(3,i,4)];

        img_ptr = &Images[idx*im_H*im_W];

        cnt0 = 0;

        if(mirror == 0)
        {
            for(x=x1; x < x1+win_W; x++)
                for(y=y1; y < y1+win_H; y++)
                    win[cnt0++] = img_ptr[INDEX(y,x,im_H)];
        } else {
            for(x=x1+win_W-1; x >= x1; x--)
                for(y=y1; y < y1+win_H; y++)
                    win[cnt0++] = img_ptr[INDEX(y,x,im_H)];
        }
        liblbp_pyr_features(&Features[nDim*i], nDim, win, win_H, win_W);
    }
    free(win);
    
    Psi->data = Features;
}

void flandmark_getPsiMatSparse(FLANDMARK_PSI_SPARSE* Psi, FLANDMARK_Model* model, int lbpidx)
{
    t_index * Features;
    uint8_t * Images = model->normalizedImageFrame;
    uint32_t im_H = (uint32_t)model->data.imSize[0]; 
    uint32_t im_W = (uint32_t)model->data.imSize[1];
    uint32_t * Wins = model->data.lbp[lbpidx].wins;
    uint16_t win_H = (uint16_t)model->data.lbp[lbpidx].winSize[0]; 
    uint16_t win_W = (uint16_t)model->data.lbp[lbpidx].winSize[1];
    uint16_t nPyramids = model->data.lbp[lbpidx].hop;
    uint32_t nDim = liblbp_pyr_get_dim(win_H, win_W, nPyramids)/256; 
    uint32_t nData = model->data.lbp[lbpidx].WINS_COLS;
    
    uint32_t cnt, cnt0, mirror, x, x1, y, y1, idx, *win;
    uint8_t *img_ptr;
    
    Features = (t_index*)calloc(nDim*nData, sizeof(t_index));
    if (Features == NULL)
    {
        printf( "Not enough memory for LBP features.\n");
        exit(1);
    }

    win = (uint32_t*)calloc(win_H*win_W, sizeof(uint32_t));
    if(win == NULL)
    {
        printf( "Not enough memory for cropped_window.\n");
        exit(1);
    }
    
    cnt = 0;
    for(uint32_t i = 0; i < nData; ++i)
    {
        idx = Wins[INDEX(0,i,4)]-1;
        x1  = Wins[INDEX(1,i,4)]-1;
        y1  = Wins[INDEX(2,i,4)]-1;
        mirror = Wins[INDEX(3,i,4)];

        img_ptr = &Images[idx*im_H*im_W];

        cnt0 = 0;

        if(mirror == 0)
        {
            for(x=x1; x < x1+win_W; x++)
                for(y=y1; y < y1+win_H; y++)
                    win[cnt0++] = img_ptr[INDEX(y,x,im_H)];
        } else {
            for(x=x1+win_W-1; x >= x1; x--)
                for(y=y1; y < y1+win_H; y++)
                    win[cnt0++] = img_ptr[INDEX(y,x,im_H)];
        }
        liblbp_pyr_features_sparse(&Features[nDim*i], nDim, win, win_H, win_W);
    }
    
    Psi->PSI_COLS = nData; 
    Psi->PSI_ROWS = nDim;
    Psi->idxs = Features;
    
    free(win);
}

int flandmark_detect_base(uint8_t* face_image, FLANDMARK_Model* model, float * landmarks)
{
    const int M = model->data.options.M;
    double * W = model->W;
    int tsize = -1, cols = -1, rows = -1;
    int * mapTable = &model->data.mapTable[0];
    
    if (!model->normalizedImageFrame)
    {
        model->normalizedImageFrame = face_image;
    } else {
        //
    }

    // get PSI matrix
    FLANDMARK_PSI_SPARSE * Psi_sparse = (FLANDMARK_PSI_SPARSE*)malloc(M*sizeof(FLANDMARK_PSI_SPARSE));
    for (int idx = 0; idx < M; ++idx)
    {      
        flandmark_getPsiMatSparse(&Psi_sparse[idx], model, idx);
    }
    
    // get Q and G
    double ** q = (double**)calloc(M, sizeof(double*));
    double ** g = (double**)calloc((M-1), sizeof(double*));

    int idx_qtemp = 0;

    for (int idx = 0; idx < M; ++idx)
    {
        // Q
        tsize = mapTable[INDEX(idx, 1, M)] - mapTable[INDEX(idx, 0, M)] + 1;
                
        //double * q_temp = (double*)malloc(tsize*sizeof(double));
        double * q_temp = (double*)calloc(tsize, sizeof(double));
        memcpy(q_temp, W+mapTable[INDEX(idx, 0, M)]-1, tsize*sizeof(double));

        // sparse dot product <W_q, PSI_q>
        cols = Psi_sparse[idx].PSI_COLS; rows = Psi_sparse[idx].PSI_ROWS;
        uint32_t *psi_temp = Psi_sparse[idx].idxs;
        q[idx] = (double*)malloc(cols*sizeof(double));
        for (int i = 0; i < cols; ++i)
        {
            double dotprod = 0.0f;
            for (int j = 0; j < rows; ++j)
            {
                idx_qtemp = psi_temp[(rows*i) + j];
                dotprod += q_temp[ idx_qtemp ];
            }
            q[idx][i] = dotprod;
        }
        free(q_temp);

        // G
        if (idx > 0)
        {
            tsize = mapTable[INDEX(idx, 3, M)] - mapTable[INDEX(idx, 2, M)] + 1;
            g[idx - 1] = (double*)malloc(tsize*sizeof(double));
            memcpy(g[idx - 1], W+mapTable[INDEX(idx, 2, M)]-1, tsize*sizeof(double));
        }
    }

    // compute argmax
    int * indices = (int*)malloc(M*sizeof(int));

    tsize = mapTable[INDEX(1, 3, M)] - mapTable[INDEX(1, 2, M)] + 1;

    // left branch - store maximum and index of s5 for all positions of s1
    int q1_length = Psi_sparse[1].PSI_COLS;
    double * s1 = (double *)calloc(2*q1_length, sizeof(double));
    double * s1_maxs = (double *)calloc(q1_length, sizeof(double));
    for (int i = 0; i < q1_length; ++i)
    {
        // dot product <g_5, PsiGS1>
        maximize_gdotprod(
            //s2_maxs, s2_idxs,
            &s1[INDEX(0, i, 2)], (double*)&s1[INDEX(1, i, 2)],
            q[5], g[4], model->data.options.PsiGS1[INDEX(i, 0, model->data.options.PSIG_ROWS[1])].disp,
            model->data.options.PsiGS1[INDEX(i, 0, model->data.options.PSIG_ROWS[1])].COLS, tsize
        );
        s1[INDEX(0, i, 2)] += q[1][i];
    }
    for (int i = 0; i < q1_length; ++i)
    {
        s1_maxs[i] = s1[INDEX(0, i, 2)];
    }

    // right branch (s2->s6) - store maximum and index of s6 for all positions of s2
    int q2_length = Psi_sparse[2].PSI_COLS;
    double * s2 = (double *)calloc(2*q2_length, sizeof(double));
    double * s2_maxs = (double *)calloc(q2_length, sizeof(double));
    for (int i = 0; i < q2_length; ++i)
    {
        // dot product <g_6, PsiGS2>
        maximize_gdotprod(
                //s2_maxs, s2_idxs,
                &s2[INDEX(0, i, 2)], (double*)&s2[INDEX(1, i, 2)],
                q[6], g[5], model->data.options.PsiGS2[INDEX(i, 0, model->data.options.PSIG_ROWS[2])].disp,
                model->data.options.PsiGS2[INDEX(i, 0, model->data.options.PSIG_ROWS[2])].COLS, tsize);
        s2[INDEX(0, i, 2)] += q[2][i];
    }
    for (int i = 0; i < q2_length; ++i)
    {
        s2_maxs[i] = s2[INDEX(0, i, 2)];
    }

    // the root s0 and its connections
    int q0_length = Psi_sparse[0].PSI_COLS;
    double maxs0 = -FLT_MAX; int maxs0_idx = -1;
    double maxq10 = -FLT_MAX, maxq20 = -FLT_MAX, maxq30 = -FLT_MAX, maxq40 = -FLT_MAX, maxq70 = -FLT_MAX;
    double * s0 = (double *)calloc(M*q0_length, sizeof(double));
    for (int i = 0; i < q0_length; ++i)
    {
        // q10
        maxq10 = -FLT_MAX;
        maximize_gdotprod(
                &maxq10, &s0[INDEX(1, i, M)],
                s1_maxs, g[0], model->data.options.PsiGS0[INDEX(i, 0, model->data.options.PSIG_ROWS[0])].disp,
                model->data.options.PsiGS0[INDEX(i, 0, model->data.options.PSIG_ROWS[0])].COLS, tsize);
        s0[INDEX(5, i, M)] = s1[INDEX(1, (int)s0[INDEX(1, i, M)], 2)];
        // q20
        maxq20 = -FLT_MAX;
        maximize_gdotprod(
                &maxq20, &s0[INDEX(2, i, M)],
                s2_maxs, g[1], model->data.options.PsiGS0[INDEX(i, 1, model->data.options.PSIG_ROWS[0])].disp,
                model->data.options.PsiGS0[INDEX(i, 1, model->data.options.PSIG_ROWS[0])].COLS, tsize);
        s0[INDEX(6, i, M)] = s2[INDEX(1, (int)s0[INDEX(2, i, M)], 2)];
        // q30
        maxq30 = -FLT_MAX;
        maximize_gdotprod(
                &maxq30, &s0[INDEX(3, i, M)],
                q[3], g[2], model->data.options.PsiGS0[INDEX(i, 2, model->data.options.PSIG_ROWS[0])].disp,
                model->data.options.PsiGS0[INDEX(i, 2, model->data.options.PSIG_ROWS[0])].COLS, tsize);
        // q40
        maxq40 = -FLT_MAX;
        maximize_gdotprod(
                &maxq40, &s0[INDEX(4, i, M)],
                q[4], g[3], model->data.options.PsiGS0[INDEX(i, 3, model->data.options.PSIG_ROWS[0])].disp,
                model->data.options.PsiGS0[INDEX(i, 3, model->data.options.PSIG_ROWS[0])].COLS, tsize);
        // q70
        maxq70 = -FLT_MAX;
        maximize_gdotprod(
                &maxq70, &s0[INDEX(7, i, M)],
                q[7], g[6], model->data.options.PsiGS0[INDEX(i, 4, model->data.options.PSIG_ROWS[0])].disp,
                model->data.options.PsiGS0[INDEX(i, 4, model->data.options.PSIG_ROWS[0])].COLS, tsize);
        // sum q10+q20+q30+q40+q70
        if (maxs0 < maxq10+maxq20+maxq30+maxq40+maxq70+q[0][i])
        {
            maxs0_idx = i;
            s0[INDEX(0, i, M)] = i;
            maxs0 = maxq10+maxq20+maxq30+maxq40+maxq70+q[0][i];
        }
    }

    // get indices
    for (int i = 0; i < M; ++i)
    {
        indices[i] = (int)s0[INDEX(0, maxs0_idx, M)+i]+1;
    }

    // cleanup temp variables
    free(s0);
    free(s1); free(s1_maxs);
    free(s2); free(s2_maxs);

    // cleanup Psi_sparse[].idxs
    for (int i = 0; i < M; ++i)
    {
        free(Psi_sparse[i].idxs);
    }
    free(Psi_sparse);

    // cleanup q
    for (int i = 0; i < M; ++i)
    {
        free(q[i]);
    }
    free(q);
    // cleanup g
    for (int i = 0; i < M - 1; ++i)
    {
        free(g[i]);
    }
    free(g);

    // convert 1D indices to 2D coordinates of estimated positions
    int * optionsS = &model->data.options.S[0];
    for (int i = 0; i < M; ++i)
    {
        int rows = optionsS[INDEX(3, i, 4)] - optionsS[INDEX(1, i, 4)] + 1;
        landmarks[INDEX(0, i, 2)] = float(COL(indices[i], rows) + optionsS[INDEX(0, i, 4)]);
        landmarks[INDEX(1, i, 2)] = float(ROW(indices[i], rows) + optionsS[INDEX(1, i, 4)]);
    }
    free(indices);
    
    return 0;
}

int flandmark_detect(IplImage *img, int *bbox, FLANDMARK_Model *model, float *landmarks, int *bw_margin)
{
    if (bw_margin)
    {
        model->data.options.bw_margin[0] = bw_margin[0];
        model->data.options.bw_margin[1] = bw_margin[1];
    }

    // Get normalized image frame
    getNormalizedImageFrame(img, bbox, model->bb, model->normalizedImageFrame, model);
    // Call flandmark_detect_base
    flandmark_detect_base(model->normalizedImageFrame, model, landmarks);
    // transform coordinates of detected landmarks from normalized image frame back to the original image
    model->sf[0] = (float)(model->bb[2]-model->bb[0])/model->data.options.bw[0];
    model->sf[1] = (float)(model->bb[3]-model->bb[1])/model->data.options.bw[1];
    for (int i = 0; i < 2*model->data.options.M; i += 2)
    {
        landmarks[i]   = landmarks[i]*model->sf[0] + model->bb[0];
        landmarks[i+1] = landmarks[i+1]*model->sf[1] + model->bb[1];
    }

    return 0;
}

void maximize_gdotprod(double * maximum, double * idx, double * first, double * second, int * third, int cols, int tsize)
{
    *maximum = -FLT_MAX;
    *idx = -1;
    for (int dp_i = 0; dp_i < cols; ++dp_i)
    {
        double dotprod = 0.0f;
        for (int dp_j = 0; dp_j < tsize; ++dp_j)
        {
            dotprod += second[dp_j]*(double)(third[dp_i*tsize+dp_j]);   
        }
        if (*maximum < first[dp_i]+dotprod)
        {
            *idx = dp_i;
            *maximum = first[dp_i]+dotprod;
        }
    }
}

int imCrop(IplImage *input, IplImage *output, const CvRect region, FLANDMARK_Model *model)
{
    if (input->width <= 0 || input->height <= 0 || region.width <= 0 || region.height <= 0)
    {
        return 1;
    }

    if (input->depth != IPL_DEPTH_8U)
    {
        return 1;
    }

    cvSetImageROI(input, region);
    if (model->p_width < region.width || model->p_height < region.height)
    {
        cvReleaseImage(&output);
        model->p_width = region.width;
        model->p_height = region.height;
        output = cvCreateImage(cvSize(model->p_width, model->p_height), IPL_DEPTH_8U, 1);
    } else {
        output->width = region.width;
        output->height = region.height;
    }
    cvCopy(input, output, NULL);
    cvResetImageROI(input);

    return 0;
}

int imResize(IplImage *input, IplImage *output, int width, int height, FLANDMARK_Model *model)
{
    int origWidth;
    int origHeight;

    if (input)
    {
        origWidth = input->width;
        origHeight = input->height;
    }

    if (width <= 0 || height <= 0 || input == 0 || origWidth <= 0 || origHeight <= 0)
    {
        return 1;
    }

    // check output image
    if (model->p_width < width || model->p_height < height)
    {
        cvReleaseImage(&output);
        model->p_width = width;
        model->p_height = height;
        output = cvCreateImage(cvSize(model->p_width, model->p_height), IPL_DEPTH_8U, 1);
    } else {
        output->width = width;
        output->height = height;
    }

    if (width > input->width && height > input->height)
    {
        // Make the image larger
        cvResetImageROI(input);
        //cvResize(input, output, CV_INTER_LINEAR);
        cvResize(input, output, CV_INTER_CUBIC);
    } else {
        // Make the image smaller
        cvResetImageROI(input);
        //cvResize(input, output, CV_INTER_AREA);
        cvResize(input, output, CV_INTER_CUBIC);
    }

    return 0;
}

int getNormalizedImageFrame(IplImage *input, const int bbox[], double *bb, uint8_t *face_img, FLANDMARK_Model *model)
{
    bool flag;
    int d[2];
    double c[2], nd[2];

    // extend bbox by bw_margin
    d[0] = bbox[2]-bbox[0]+1;  d[1] = bbox[3]-bbox[1]+1;
    c[0] = (bbox[2]+bbox[0])/2.0f; c[1] = (bbox[3]+bbox[1])/2.0f;
    nd[0] = d[0]*model->data.options.bw_margin[0]/100.0f + d[0];
    nd[1] = d[1]*model->data.options.bw_margin[1]/100.0f + d[1];

    bb[0] = (c[0] - nd[0]/2.0f);
    bb[1] = (c[1] - nd[1]/2.0f);
    bb[2] = (c[0] + nd[0]/2.0f);
    bb[3] = (c[1] + nd[1]/2.0f);

    flag = bb[0] > 0 && bb[1] > 0 && bb[2] < input->width && bb[3] < input->height
               && bbox[0] > 0 && bbox[1] > 0 && bbox[2] < input->width && bbox[3] < input->height;

    if (!flag)
    {
        return 1;
    }

    // crop and resize image to normalized frame
    if(imCrop(input, model->croppedImage, cvRect(bb[0], bb[1], bb[2]-bb[0]+1, bb[3]-bb[1]+1), model))
    {
        // something was bad
        return 1;
    }
    // imresize
    if(imResize(model->croppedImage, model->resizedImage, model->data.options.bw[0], model->data.options.bw[1], model))
    {
        // something was bad
        return 1;
    }

    // tranform IplImage to simple 1D uint8 array representing 2D uint8 normalized image frame
    for (int x = 0; x < model->data.options.bw[0]; ++x)
    {
      for (int y = 0; y < model->data.options.bw[1]; ++y)
      {
          face_img[INDEX(x, y, model->data.options.bw[1])] = (uint8_t)((model->resizedImage->imageData + model->resizedImage->widthStep*x)[y]);
      }
    }

    return 0;
}
