/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2012 Michal Uricar
 * Copyright (C) 2012 Michal Uricar
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>
#include <stdexcept>
#include <boost/format.hpp>
#include <bob.ip.base/Affine.h>

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

namespace bob { namespace ip { namespace flandmark {

void flandmark_write_model(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)
  {
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
  }

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

  //int fscan_ret = -1;
  if (fscanf(fin, " %c ", &tst->data.options.M) < 1)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());

  if (fscanf(fin, " %d %d ", &tst->data.options.bw[0], &tst->data.options.bw[1]) < 2)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());

  if (fscanf(fin, " %d %d ", &tst->data.options.bw_margin[0], &tst->data.options.bw_margin[1]) < 2)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());

  if (fscanf(fin, " %d %d ", &tst->W_ROWS, &tst->W_COLS) < 2)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());

  if (fscanf(fin, " %d %d ", &tst->data.imSize[0], &tst->data.imSize[1]) < 2)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());

  int M = tst->data.options.M;

  tst->data.lbp = (FLANDMARK_LBP*)malloc(M*sizeof(FLANDMARK_LBP));
  for (int idx = 0; idx < M; ++idx){
    if (fscanf(fin, " %d %d ", &tst->data.lbp[idx].WINS_ROWS, &tst->data.lbp[idx].WINS_COLS) < 2)
      throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
  }

  for (int idx = 0; idx < 3; ++idx){
    if (fscanf(fin, " %d %d ", &tst->data.options.PSIG_ROWS[idx], &tst->data.options.PSIG_COLS[idx]) < 2)
      throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
  }

  // load model.W -----------------------------------------------------------
  tst->W = (double*)malloc(tst->W_ROWS * sizeof(double));
  if (fread(tst->W, tst->W_ROWS * sizeof(double), 1, fin) != 1)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());

  // load model.data.mapTable -----------------------------------------------
  p_int = (int*)malloc(M*4*sizeof(int));
  tst->data.mapTable = (int*)malloc(M*4*sizeof(int));
  if (fread(p_int, M*4*sizeof(int), 1, fin) != 1)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
  for (int i = 0; i < M*4; ++i)
    tst->data.mapTable[i] = p_int[i];
  free(p_int);

  // load model.data.lbp ---------------------------------------------------
  for (int idx = 0; idx < M; ++idx){
    // lbp{idx}.winSize
    p_int = (int*)malloc(2*sizeof(int));
    if (fread(p_int, 2*sizeof(int), 1, fin) != 1)
      throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
    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)
      throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
    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)
      throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
  }

  // load model.options.S --------------------------------------------------
  p_int = (int*)malloc(4*M*sizeof(int));
  tst->data.options.S = (int*)malloc(4*M*sizeof(int));
  if (fread(p_int, 4*M*sizeof(int), 1, fin) != 1)
    throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
  for (int i = 0; i < 4*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)
        throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
      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)
        throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
      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)
        throw std::runtime_error((boost::format("Error opening file %s") % filename).str().c_str());
    }
  }

  fclose(fin);

  tst->normalizedImageFrame = (uint8_t*)calloc(tst->data.options.bw[0]*tst->data.options.bw[1], sizeof(uint8_t));

  return tst;
}

EError_T flandmark_check_model(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[%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->normalizedImageFrame)
    free(model->normalizedImageFrame);

  free(model);
}

void flandmark_get_psi_mat(FLANDMARK_PSI* Psi, FLANDMARK_Model* model, int lbpidx)
{
  char * Features;
  const 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];
  const 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)
    throw std::runtime_error("Not enough memory for LBP features.");
  Psi->PSI_ROWS = nDim; Psi->PSI_COLS = nData;

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

  win = (uint32_t*)malloc(win_H*win_W*sizeof(uint32_t));
  if(win == NULL)
    throw std::runtime_error("Not enough memory for cropped_window.");

  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_get_psi_mat_sparse(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 cnt0, mirror, x, x1, y, y1, idx, *win;
  uint8_t *img_ptr;

  Features = (t_index*)calloc(nDim*nData, sizeof(t_index));
  if (Features == NULL)
    throw std::runtime_error("Not enough memory for LBP features.");

  win = (uint32_t*)calloc(win_H*win_W, sizeof(uint32_t));
  if(win == NULL)
    throw std::runtime_error("Not enough memory for cropped_window.");

  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);
}

void flandmark_argmax(double *smax, FLANDMARK_Options *options, const int *mapTable, FLANDMARK_PSI_SPARSE *Psi_sparse, double **q, double **g)
{
    uint8_t M = options->M;

    // compute argmax
    int * indices = (int*)malloc(M*sizeof(int));
    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>
      flandmark_maximize_gdotprod(
              //s2_maxs, s2_idxs,
              &s1[INDEX(0, i, 2)], (double*)&s1[INDEX(1, i, 2)],
              q[5], g[4], options->PsiGS1[INDEX(i, 0, options->PSIG_ROWS[1])].disp,
              options->PsiGS1[INDEX(i, 0, 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>
      flandmark_maximize_gdotprod(
              //s2_maxs, s2_idxs,
              &s2[INDEX(0, i, 2)], (double*)&s2[INDEX(1, i, 2)],
              q[6], g[5], options->PsiGS2[INDEX(i, 0, options->PSIG_ROWS[2])].disp,
              options->PsiGS2[INDEX(i, 0, 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;
        flandmark_maximize_gdotprod(
                &maxq10, &s0[INDEX(1, i, M)],
                s1_maxs, g[0], options->PsiGS0[INDEX(i, 0, options->PSIG_ROWS[0])].disp,
                options->PsiGS0[INDEX(i, 0, 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;
        flandmark_maximize_gdotprod(
                &maxq20, &s0[INDEX(2, i, M)],
                s2_maxs, g[1], options->PsiGS0[INDEX(i, 1, options->PSIG_ROWS[0])].disp,
                options->PsiGS0[INDEX(i, 1, 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;
        flandmark_maximize_gdotprod(
                &maxq30, &s0[INDEX(3, i, M)],
                q[3], g[2], options->PsiGS0[INDEX(i, 2, options->PSIG_ROWS[0])].disp,
                options->PsiGS0[INDEX(i, 2, options->PSIG_ROWS[0])].COLS, tsize);
        // q40
        maxq40 = -FLT_MAX;
        flandmark_maximize_gdotprod(
                &maxq40, &s0[INDEX(4, i, M)],
                q[4], g[3], options->PsiGS0[INDEX(i, 3, options->PSIG_ROWS[0])].disp,
                options->PsiGS0[INDEX(i, 3, options->PSIG_ROWS[0])].COLS, tsize);
        // q70
        maxq70 = -FLT_MAX;
        flandmark_maximize_gdotprod(
                &maxq70, &s0[INDEX(7, i, M)],
                q[7], g[6], options->PsiGS0[INDEX(i, 4, options->PSIG_ROWS[0])].disp,
                options->PsiGS0[INDEX(i, 4, 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);

    // convert 1D indices to 2D coordinates of estimated positions
    //int * optionsS = &options->S[0];
    const int * optionsS = options->S;
    for (int i = 0; i < M; ++i){
        int rows = optionsS[INDEX(3, i, 4)] - optionsS[INDEX(1, i, 4)] + 1;
        // MG: changed order of returned landmarks to be (y,x)
        smax[INDEX(1, i, 2)] = float(COL(indices[i], rows) + optionsS[INDEX(0, i, 4)]);
        smax[INDEX(0, i, 2)] = float(ROW(indices[i], rows) + optionsS[INDEX(1, i, 4)]);
    }
    free(indices);
}

void flandmark_detect_base(uint8_t* face_image, FLANDMARK_Model* model, double* landmarks)
{
  const int M = model->data.options.M;
  const double * W = model->W;
  int tsize = -1, cols = -1, rows = -1;
  const int * mapTable = model->data.mapTable;

  if (!model->normalizedImageFrame)
  {
    model->normalizedImageFrame = face_image;
  }

  // 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_get_psi_mat_sparse(&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*)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));
    }
  }

    // argmax
    flandmark_argmax(landmarks, &model->data.options, mapTable, Psi_sparse, q, g);

  // 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);

}

void flandmark_detect(const blitz::Array<uint8_t,2>& image, int* bbox, FLANDMARK_Model *model, double *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];
  }

  int corrected_bbx[4];

  // Get normalized image frame
  flandmark_get_normalized_image_frame(image, bbox, corrected_bbx, 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
  double sx = double(corrected_bbx[3] - corrected_bbx[1]) / model->data.options.bw[0];
  double sy = double(corrected_bbx[2] - corrected_bbx[0]) / model->data.options.bw[1];
  for (int i = 0; i < 2*model->data.options.M; i+=2)
  {
    landmarks[i]   = landmarks[i]*sy + corrected_bbx[0];
    landmarks[i+1] = landmarks[i+1]*sx + corrected_bbx[1];
  }
}

void flandmark_maximize_gdotprod(double * maximum, double * idx, const double * first, const double * second, const int * third, const int cols, const 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;
    }
  }
}

void flandmark_imcrop(const blitz::Array<uint8_t, 2>& input, blitz::Array<uint8_t,2>& output, int* bbx)
{
  if (bbx[0] < 0 or bbx[2] >= input.extent(0) or bbx[1] < 0 or bbx[3] >= input.extent(1))
    throw std::runtime_error("Bounding box exceeds image resolution");

  output.resize(bbx[2]-bbx[0], bbx[3]-bbx[1]);
  blitz::Range a = blitz::Range::all();
  output(a,a) = input(blitz::Range(bbx[0], bbx[2]-1), blitz::Range(bbx[1], bbx[3]-1));
}

void flandmark_get_normalized_image_frame(const blitz::Array<uint8_t, 2>& input, const int bbox[], int *corrected_bbx, uint8_t *face_img, FLANDMARK_Model *model)
{
  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.;
  c[1] = (bbox[3]+bbox[1])/2.;
  nd[0] = d[0]*model->data.options.bw_margin[1]/100. + d[0];
  nd[1] = d[1]*model->data.options.bw_margin[0]/100. + d[1];

  corrected_bbx[0] = int(c[0] - nd[0]/2.) + 2;
  corrected_bbx[1] = int(c[1] - nd[1]/2.) + 2;
  corrected_bbx[2] = int(c[0] + nd[0]/2.);
  corrected_bbx[3] = int(c[1] + nd[1]/2.);

  blitz::Array<uint8_t, 2> croppedImage;
  blitz::Array<double, 2> scaledImage(model->data.options.bw[1], model->data.options.bw[0]);

  flandmark_imcrop(input, croppedImage, corrected_bbx);
  bob::ip::base::scale(croppedImage, scaledImage);

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

}

} } } // namespace bob ip flandmark
