#include "Recognize.h"
#include "edges.h"
#include "AlgUtils.h"
#include "tsai.h"

#ifdef WIN32
#include <windows.h>
#endif
#include <GL/gl.h>
#include <stdio.h>
#include <math.h>

#ifndef SQR
#define SQR(a) ((a)*(a))
#endif

static TreeInterpLinha linha[40];
static int nlinhas=0;
static int nRecogLinhas=0;

#ifdef MODELO_1
#define NPONTOS_MDL 16
#define NLINHAS 8
#else
#define NPONTOS_MDL 15
#define NLINHAS 9
#endif

#define MAX_POINTS	128

static TreeInterpLinha LINHA[NLINHAS];
static unsigned char *ResInterpret = NULL;

GeomPonto2D PontosMdl[NPONTOS_MDL];
GeomPonto2D PontosReconhecidos[NPONTOS_MDL];
bool validPoints[NPONTOS_MDL];

RacLine lines[NLINHAS];
bool validLines[NLINHAS];

static int gWidth = 320;
static int gHeight = 240;

#define NEARPLANE  10
#define FARPLANE  500

static double xw[NPONTOS_MDL];
static double yw[NPONTOS_MDL];
static double xp[NPONTOS_MDL];
static double yp[NPONTOS_MDL];
static int nRecogPoints;

static TreeInterpLinha lr[NLINHAS];
static TreeInterpLinha lr0, lr1, lr2, lr3, lr4, lr5, lr6, lr7, lr8;
static GeomPonto2D p_ar[NLINHAS][1000];
static unsigned char ppeso_ar[NLINHAS][1000];
static float A00, A01, A02, A10, A11, A12, A20, A21, A22;
static float B00, B01, B02, B10, B11, B12, B20, B21, B22;
static GeomPonto3D ptfg[2];
/*
static void EncontraPtsFuga()
{
 TreeInterpLinha _linhai[NLINHAS];
 unsigned char f[NLINHAS];// = {0, 0, 0, 0, 0, 0, 0, 0, 0};
 for (int k = 0; k < NLINHAS; k++)
   f[k] = 0;

 for (unsigned char i=0, *pR=ResInterpret; i<nlinhas; i++, pR++)
  if (*pR != 255) {
   f[*pR] = 1;
   _linhai[*pR] = linha[i];
  }

 float A[2*MAX_POINTS][2];
 float b[2*MAX_POINTS];

 int cont, l=1;
 if (f[1]) {
  b[0] = ExtraiCoef(_linhai[1], A[0][0], A[0][1]);
  cont = 1;
 } else cont = 0;
 if (f[3]) {
  b[cont] = ExtraiCoef(_linhai[3], A[cont][0], A[cont][1]);
  cont++;
  l = 3;
 }
 if (f[4]) {
  b[cont] = ExtraiCoef(_linhai[4], A[cont][0], A[cont][1]);
  cont++;
  l = 4;
 }
 if (f[6]) {
  b[cont] = ExtraiCoef(_linhai[6], A[cont][0], A[cont][1]);
  cont++;
  l = 6;
 }
 if (ptfg[0].z = MinQuad(cont, A, b)) {
  ptfg[0].x = b[0];
  ptfg[0].y = b[1];
 } else {
  float x = _linhai[l].dx, y = _linhai[l].dy, n = (float)sqrt(x*x + y*y);
  ptfg[0].x = x/n;
  ptfg[0].y = y/n;
 }

 b[0] = ExtraiCoef(_linhai[0], A[0][0], A[0][1]);
 b[1] = ExtraiCoef(_linhai[2], A[1][0], A[1][1]);
 b[2] = ExtraiCoef(_linhai[5], A[2][0], A[2][1]);
 if (ptfg[1].z = MinQuad(3, A, b)) {
  ptfg[1].x = b[0];
  ptfg[1].y = b[1];
 } else {
  float x = _linhai[0].dx, y = _linhai[0].dy, n = (float)sqrt(x*x + y*y);
  ptfg[1].x = x/n;
  ptfg[1].y = y/n;
 }
}*/

static int AjustaReconhecimento(unsigned char *imagem, int tol)
{
 int i, j, k;
 int np_ar[NLINHAS];// = {0,0,0,0,0,0,0,0,0};
 for (i=0;i<NLINHAS;i++)
   np_ar[i]=0;

 float minx = gWidth, miny = gHeight, maxx = 0, maxy = 0;
 TreeInterpLinha *plr=lr, *plrmax=plr+NLINHAS-1;
 int tol2 = tol*tol;

 for (TreeInterpLinha *L=LINHA; plr<=plrmax; plr++, L++) {
  float w = A20*L->v1x + A21*L->v1y + A22;
  plr->v1x = (A00*L->v1x + A01*L->v1y + A02)/w;
  plr->v1y = (A10*L->v1x + A11*L->v1y + A12)/w;
  w = A20*L->v2x + A21*L->v2y + A22;
  plr->v2x = (A00*L->v2x + A01*L->v2y + A02)/w;
  plr->v2y = (A10*L->v2x + A11*L->v2y + A12)/w;
  if (plr->v1x < plr->v2x) {
   if ((plr->minx = plr->v1x-tol) < minx) minx = plr->minx;
   if ((plr->maxx = plr->v2x+tol) > maxx) maxx = plr->maxx;
  } else {
   if ((plr->minx = plr->v2x-tol) < minx) minx = plr->minx;
   if ((plr->maxx = plr->v1x+tol) > maxx) maxx = plr->maxx;
  }
  if (plr->v1y < plr->v2y) {
   if ((plr->miny = plr->v1y-tol) < miny) miny = plr->miny;
   if ((plr->maxy = plr->v2y+tol) > maxy) maxy = plr->maxy;
  } else {
   if ((plr->miny = plr->v2y-tol) < miny) miny = plr->miny;
   if ((plr->maxy = plr->v1y+tol) > maxy) maxy = plr->maxy;
  }
  plr->dx  = plr->v2x - plr->v1x,
  plr->dy  = plr->v2y - plr->v1y;
  plr->compr2 = plr->dx*plr->dx + plr->dy*plr->dy;
  plr->tol = plr->compr2*tol2;
  plr->aux = plr->dx*(plr->v1y+plr->v2y);
 }

 int xm = (int)((minx < 1)? 1 : minx),
     ym = (int)((miny < 1)? 1 : miny),
     xM = (int)((maxx > gWidth-1)? gWidth-1 : maxx),
     yM = (int)((maxy > gHeight-1)? gHeight-1 : maxy);
 int *pnp, *pnp_aux;
 unsigned char *pi, *pi_aux = &imagem[ym*gWidth];
 for (j=ym; j<=yM; j++, pi_aux += gWidth)
 {
  for (i=xm, pi=pi_aux+i; i<=xM; i++, pi++)
   if (*pi) {
    float m=99999, maux;
    int nl=-1;
    for (k=0, plr=lr, pnp=np_ar; k<NLINHAS; k++, plr++, pnp++)
     if (i>plr->minx && i<plr->maxx && j>plr->miny && j<plr->maxy) {
      float px_=i - plr->v1x,
            area=(plr->dx-px_)*(plr->v2y+j) + px_*(j+plr->v1y) - plr->aux;
      if ((area=SQR(area)) > plr->tol) maux=999999;
      else {
       float t = plr->dx*px_ + plr->dy*(j-plr->v1y);
       maux = (t<0 || t>plr->compr2)? 999999 : area/plr->compr2;
      }
      if (maux<m) {
       nl = k;
       pnp_aux = pnp;
       if ((m=maux) == 0) break;
      }
     }
    if (nl != -1) {
     p_ar[nl][*pnp_aux].x = (float)i;
     p_ar[nl][*pnp_aux].y = (float)j;
     ppeso_ar[nl][*pnp_aux] = *pi;
     (*pnp_aux)++;
    }
   }
 }

 for (k=0, pnp=np_ar, plr=lr; k<NLINHAS; k++, pnp++, plr++)
 {
  if (*pnp > 1) {
   ExtraiLinha(p_ar[k], ppeso_ar[k], *pnp, *plr);
   plr->dx = plr->v2x - plr->v1x,
   plr->dy = plr->v2y - plr->v1y;
   plr->valido = 1;
  } else plr->valido = 0;
 }
// lr0=lr[0], lr1=lr[1], lr2=lr[2], lr3=lr[3], lr4=lr[4], lr5=lr[5], lr6=lr[6], lr7=lr[7], lr8=lr[8];
 lr0=lr[0], lr1=lr[1], lr2=lr[2], lr3=lr[3], lr4=lr[4], lr5=lr[5], lr6=lr[6], lr7=lr[7], lr8=lr[8];
 return 1;
}

static int AjustaReconhecimento(unsigned char *imagem, int tol, float lambda)
{
 int i, j, k;
 int np_ar[NLINHAS];// = {0,0,0,0,0,0,0};
 for (i=0;i<NLINHAS;i++)
   np_ar[i]=0;

 float minx = gWidth, miny = gHeight, maxx = 0, maxy = 0;
 TreeInterpLinha *plr=lr, *plrmax=plr+NLINHAS-1;
 int tol2 = tol*tol;

 float l1_v1x, l1_v1y, l1_v2x, l1_v2y;
 float l2_v1x, l2_v1y, l2_v2x, l2_v2y;
 for (TreeInterpLinha *L=LINHA; plr<=plrmax; plr++, L++) {
  float w = A20*L->v1x + A21*L->v1y + A22;
  l1_v1x = (A00*L->v1x + A01*L->v1y + A02)/w;
  l1_v1y = (A10*L->v1x + A11*L->v1y + A12)/w;
  w = A20*L->v2x + A21*L->v2y + A22;
  l1_v2x = (A00*L->v2x + A01*L->v2y + A02)/w;
  l1_v2y = (A10*L->v2x + A11*L->v2y + A12)/w;
  w = B20*L->v1x + B21*L->v1y + B22;
  l2_v1x = (B00*L->v1x + B01*L->v1y + B02)/w;
  l2_v1y = (B10*L->v1x + B11*L->v1y + B12)/w;
  w = B20*L->v2x + B21*L->v2y + B22;
  l2_v2x = (B00*L->v2x + B01*L->v2y + B02)/w;
  l2_v2y = (B10*L->v2x + B11*L->v2y + B12)/w;
  plr->v1x = lambda*(2*l2_v1x - l1_v1x) + (1-lambda)*l2_v1x;
  plr->v1y = lambda*(2*l2_v1y - l1_v1y) + (1-lambda)*l2_v1y;
  plr->v2x = lambda*(2*l2_v2x - l1_v2x) + (1-lambda)*l2_v2x;
  plr->v2y = lambda*(2*l2_v2y - l1_v2y) + (1-lambda)*l2_v2y;
  if (plr->v1x < plr->v2x) {
   if ((plr->minx = plr->v1x-tol) < minx) minx = plr->minx;
   if ((plr->maxx = plr->v2x+tol) > maxx) maxx = plr->maxx;
  } else {
   if ((plr->minx = plr->v2x-tol) < minx) minx = plr->minx;
   if ((plr->maxx = plr->v1x+tol) > maxx) maxx = plr->maxx;
  }
  if (plr->v1y < plr->v2y) {
   if ((plr->miny = plr->v1y-tol) < miny) miny = plr->miny;
   if ((plr->maxy = plr->v2y+tol) > maxy) maxy = plr->maxy;
  } else {
   if ((plr->miny = plr->v2y-tol) < miny) miny = plr->miny;
   if ((plr->maxy = plr->v1y+tol) > maxy) maxy = plr->maxy;
  }
  plr->dx  = plr->v2x - plr->v1x,
  plr->dy  = plr->v2y - plr->v1y;
  plr->compr2 = plr->dx*plr->dx + plr->dy*plr->dy;
  plr->tol = plr->compr2*tol2;
  plr->aux = plr->dx*(plr->v1y+plr->v2y);
 }

 int xm = (int)((minx < 1)? 1 : minx),
     ym = (int)((miny < 1)? 1 : miny),
     xM = (int)((maxx > gWidth-1)? gWidth-1 : maxx),
     yM = (int)((maxy > gHeight-1)? gHeight-1 : maxy);
 int *pnp, *pnp_aux;
 unsigned char *pi, *pi_aux = &imagem[ym*gWidth];
 for (j=ym; j<=yM; j++, pi_aux += gWidth)
  for (i=xm, pi=pi_aux+i; i<=xM; i++, pi++)
   if (*pi) {
    float m=99999, maux;
    int nl=-1;
    for (k=0, plr=lr, pnp=np_ar; k<NLINHAS; k++, plr++, pnp++)
     if (i>plr->minx && i<plr->maxx && j>plr->miny && j<plr->maxy) {
      float px_=i - plr->v1x,
            area=(plr->dx-px_)*(plr->v2y+j) + px_*(j+plr->v1y) - plr->aux;
      if ((area=SQR(area)) > plr->tol) maux=999999;
      else {
       float t = plr->dx*px_ + plr->dy*(j-plr->v1y);
       maux = (t<0 || t>plr->compr2)? 999999 : area/plr->compr2;
      }
      if (maux<m) {
       nl = k;
       pnp_aux = pnp;
       if ((m=maux) == 0) break;
      }
     }
    if (nl != -1) {
     p_ar[nl][*pnp_aux].x = (float)i;
     p_ar[nl][*pnp_aux].y = (float)j;
     ppeso_ar[nl][*pnp_aux] = *pi;
     (*pnp_aux)++;
    }
   }

 for (k=0, pnp=np_ar, plr=lr; k<NLINHAS; k++, pnp++, plr++) 
  if (*pnp > 1) {
   ExtraiLinha(p_ar[k], ppeso_ar[k], *pnp, *plr);
   plr->dx = plr->v2x - plr->v1x,
   plr->dy = plr->v2y - plr->v1y;
   plr->valido = 1;
  } else plr->valido = 0;
 lr0=lr[0], lr1=lr[1], lr2=lr[2], lr3=lr[3], lr4=lr[4], lr5=lr[5], lr6=lr[6], lr7=lr[7], lr8=lr[8];
 return 1;
}

static float ErroMax(double *X, int n, double* xw, double* yw, double* xp, double* yp)
{
 float _ErroMax = 0.0f;
 double *ppxw, *ppyw, *ppxp, *ppyp, *pp1max;
 for (ppxw=xw, ppyw=yw, ppxp=xp, ppyp=yp, pp1max=(xw+n-1); ppxw<=pp1max; ppxw++, ppyw++, ppxp++, ppyp++) {
  float w = X[6]*(*ppxw) + X[7]*(*ppyw) + X[8];
  float px = (X[0]*(*ppxw) + X[1]*(*ppyw) + X[2])/w;
  float py = (X[3]*(*ppxw) + X[4]*(*ppyw) + X[5])/w;
  float erro = (float)sqrt(SQR(px - (*ppxp))+SQR(py - (*ppyp)));
  _ErroMax = (erro > _ErroMax) ? erro : _ErroMax;
 }
 return _ErroMax;
}

static int ReconhecePontosModelo1()
{
  nRecogLinhas = 0;
  for (int i = 0; i < NLINHAS; i++)
    validLines[i] = false;

  for (int i = 0; i < nlinhas; i++)
  {
    int currline = ResInterpret[i];
    if ( currline != 255 && currline >= 0 && currline < NLINHAS)
	{
	  RacPixel p1, p2;
	  p1.x = linha[i].v1x;
	  p1.y = linha[i].v1y;
	  p2.x = linha[i].v2x;
	  p2.y = linha[i].v2y;
	  if (lineEquation(p1, p2, lines[currline]))
	  {
        validLines[currline] = true;
        nRecogLinhas++;
	  }
	}
  }
  if (nRecogLinhas > 4)
  {
    for (int i = 0; i < NPONTOS_MDL; i++)
      validPoints[i] = false;

    RacPixel currPixel;

	// Acha Pontos que interceptam a linha 0
    if (validLines[0])
	{
	  // Interseccao com linha 7
	  if (validLines[7])
	  {
	    if (linesIntersection(lines[0], lines[7], currPixel))
		{
		  PontosReconhecidos[0].x = currPixel.x;
		  PontosReconhecidos[0].y = currPixel.y;
		  validPoints[0] = true;
		}
	  }
	  // Interseccao com linha 5
	  if (validLines[5])
	  {
	    if (linesIntersection(lines[0], lines[5], currPixel))
		{
		  PontosReconhecidos[1].x = currPixel.x;
		  PontosReconhecidos[1].y = currPixel.y;
		  validPoints[1] = true;
		}
	  }
	  // Interseccao com linha 3
	  if (validLines[3])
	  {
	    if (linesIntersection(lines[0], lines[3], currPixel))
		{
		  PontosReconhecidos[2].x = currPixel.x;
		  PontosReconhecidos[2].y = currPixel.y;
		  validPoints[2] = true;
		}
	  }
	  // Interseccao com linha 1
	  if (validLines[1])
	  {
	    if (linesIntersection(lines[0], lines[1], currPixel))
		{
		  PontosReconhecidos[3].x = currPixel.x;
		  PontosReconhecidos[3].y = currPixel.y;
		  validPoints[3] = true;
		}
	  }
    }

	// Acha Pontos que interceptam a linha 4
    if (validLines[4])
	{
	  // Interseccao com linha 7
	  if (validLines[7])
	  {
	    if (linesIntersection(lines[4], lines[7], currPixel))
		{
		  PontosReconhecidos[4].x = currPixel.x;
		  PontosReconhecidos[4].y = currPixel.y;
		  validPoints[4] = true;
		}
	  }
	  // Interseccao com linha 5
	  if (validLines[5])
	  {
	    if (linesIntersection(lines[4], lines[5], currPixel))
		{
		  PontosReconhecidos[5].x = currPixel.x;
		  PontosReconhecidos[5].y = currPixel.y;
		  validPoints[5] = true;
		}
	  }
	  // Interseccao com linha 3
	  if (validLines[3])
	  {
	    if (linesIntersection(lines[4], lines[3], currPixel))
		{
		  PontosReconhecidos[6].x = currPixel.x;
		  PontosReconhecidos[6].y = currPixel.y;
		  validPoints[6] = true;
		}
	  }
	  // Interseccao com linha 1
	  if (validLines[1])
	  {
	    if (linesIntersection(lines[4], lines[1], currPixel))
		{
		  PontosReconhecidos[7].x = currPixel.x;
		  PontosReconhecidos[7].y = currPixel.y;
		  validPoints[7] = true;
		}
	  }
	}

	// Acha Pontos que interceptam a linha 6
    if (validLines[6])
	{
	  // Interseccao com linha 7
	  if (validLines[7])
	  {
	    if (linesIntersection(lines[6], lines[7], currPixel))
		{
		  PontosReconhecidos[8].x = currPixel.x;
		  PontosReconhecidos[8].y = currPixel.y;
		  validPoints[8] = true;
		}
	  }
	  // Interseccao com linha 5
	  if (validLines[5])
	  {
	    if (linesIntersection(lines[6], lines[5], currPixel))
		{
		  PontosReconhecidos[9].x = currPixel.x;
		  PontosReconhecidos[9].y = currPixel.y;
		  validPoints[9] = true;
		}
	  }
	  // Interseccao com linha 3
	  if (validLines[3])
	  {
	    if (linesIntersection(lines[6], lines[3], currPixel))
		{
		  PontosReconhecidos[10].x = currPixel.x;
		  PontosReconhecidos[10].y = currPixel.y;
		  validPoints[10] = true;
		}
	  }
	  // Interseccao com linha 1
	  if (validLines[1])
	  {
	    if (linesIntersection(lines[6], lines[1], currPixel))
		{
		  PontosReconhecidos[11].x = currPixel.x;
		  PontosReconhecidos[11].y = currPixel.y;
		  validPoints[11] = true;
		}
	  }
	}

	// Acha Pontos que interceptam a linha 2
    if (validLines[2])
	{
	  // Interseccao com linha 7
	  if (validLines[7])
	  {
	    if (linesIntersection(lines[2], lines[7], currPixel))
		{
		  PontosReconhecidos[12].x = currPixel.x;
		  PontosReconhecidos[12].y = currPixel.y;
		  validPoints[12] = true;
		}
	  }
	  // Interseccao com linha 5
	  if (validLines[5])
	  {
	    if (linesIntersection(lines[2], lines[5], currPixel))
		{
		  PontosReconhecidos[13].x = currPixel.x;
		  PontosReconhecidos[13].y = currPixel.y;
		  validPoints[13] = true;
		}
	  }
	  // Interseccao com linha 3
	  if (validLines[3])
	  {
	    if (linesIntersection(lines[2], lines[3], currPixel))
		{
		  PontosReconhecidos[14].x = currPixel.x;
		  PontosReconhecidos[14].y = currPixel.y;
		  validPoints[14] = true;
		}
	  }
	  // Interseccao com linha 1
	  if (validLines[1])
	  {
	    if (linesIntersection(lines[2], lines[1], currPixel))
		{
		  PontosReconhecidos[15].x = currPixel.x;
		  PontosReconhecidos[15].y = currPixel.y;
		  validPoints[15] = true;
		}
	  }
	}
  }
  int nPontos = 0;
  for (int i = 0; i < NPONTOS_MDL; i++)
  {
    if (validPoints[i])
	{
      xw[nPontos] = (double) PontosMdl[i].x;
      yw[nPontos] = (double) PontosMdl[i].y;
      xp[nPontos] = (double) PontosReconhecidos[i].x;
	  yp[nPontos] = (double) PontosReconhecidos[i].y;
	  nPontos++;
	}
  }
  return nPontos;
}

static int ReconhecePontosModelo1Reconstruido()
{
	/*
  nRecogLinhas = 0;
  for (int i = 0; i < NLINHAS; i++)
    validLines[i] = false;

 TreeInterpLinha _linhai[NLINHAS];
 for (unsigned char i=0, *pR=ResInterpret; i<nlinhas; i++, pR++)
   if (*pR != 255)
   {
      validLines[*pR] = true;
      _linhai[*pR] = linha[i];
      nRecogLinhas++;
   }
*/
  float e; // utilizado pela macro intersecao
  if (1)//nRecogLinhas > 4)
  {
    for (int i = 0; i < NPONTOS_MDL; i++)
      validPoints[i] = false;

	// Acha Pontos que interceptam a linha 0
	if (lr0.valido)
	{
	  // Interseccao com linha 7
	  if (lr7.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[0];
 		intersecao(lr0, lr7, p);
 	    validPoints[0] = true;
	  }
	  // Interseccao com linha 5
	  if (lr5.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[1];
 		intersecao(lr0, lr5, p);
  	    validPoints[1] = true;
	  }
	  // Interseccao com linha 3
	  if (lr3.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[2];
 		intersecao(lr0, lr3, p);
	    validPoints[2] = true;
	  }
	  // Interseccao com linha 1
	  if (lr1.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[3];
 		intersecao(lr0, lr1, p);
  	    validPoints[3] = true;
	  }
    }

	// Acha Pontos que interceptam a linha 4
    if (lr4.valido)
	{
	  // Interseccao com linha 7
	  if (lr7.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[4];
 		intersecao(lr4, lr7, p);
		validPoints[4] = true;
	  }
	  // Interseccao com linha 5
	  if (lr5.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[5];
 		intersecao(lr4, lr5, p);
		validPoints[5] = true;
	  }
	  // Interseccao com linha 3
	  if (lr3.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[6];
 		intersecao(lr4, lr3, p);
		validPoints[6] = true;
	  }
	  // Interseccao com linha 1
	  if (lr1.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[7];
 		intersecao(lr4, lr1, p);
		validPoints[7] = true;
	  }
	}

	// Acha Pontos que interceptam a linha 6
    if (lr6.valido)
	{
	  // Interseccao com linha 7
	  if (lr7.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[8];
 		intersecao(lr6, lr7, p);
		validPoints[8] = true;
	  }
	  // Interseccao com linha 5
	  if (lr5.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[9];
 		intersecao(lr6, lr5, p);
		validPoints[9] = true;
	  }
	  // Interseccao com linha 3
	  if (lr3.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[10];
 		intersecao(lr6, lr3, p);
		validPoints[10] = true;
	  }
	  // Interseccao com linha 1
	  if (lr1.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[11];
 		intersecao(lr6, lr1, p);
		validPoints[11] = true;
	  }
	}

	// Acha Pontos que interceptam a linha 2
    if (lr2.valido)
	{
	  // Interseccao com linha 7
	  if (lr7.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[12];
 		intersecao(lr2, lr7, p);
		validPoints[12] = true;
	  }
	  // Interseccao com linha 5
	  if (lr5.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[13];
 		intersecao(lr2, lr5, p);
		validPoints[13] = true;
	  }
	  // Interseccao com linha 3
	  if (lr3.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[14];
 		intersecao(lr2, lr3, p);
		validPoints[14] = true;
	  }
	  // Interseccao com linha 1
	  if (lr1.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[15];
 		intersecao(lr2, lr1, p);
		validPoints[15] = true;
	  }
	}
  }
  int nPontos = 0;
  for (int i = 0; i < NPONTOS_MDL; i++)
  {
    if (validPoints[i])
	{
      xw[nPontos] = (double) PontosMdl[i].x;
      yw[nPontos] = (double) PontosMdl[i].y;
      xp[nPontos] = (double) PontosReconhecidos[i].x;
	  yp[nPontos] = (double) PontosReconhecidos[i].y;
	  nPontos++;
	}
  }
  return nPontos;
}

static int ReconhecePontosModelo2()
{
  nRecogLinhas = 0;
  for (int i = 0; i < NLINHAS; i++)
    validLines[i] = false;

 TreeInterpLinha _linhai[NLINHAS];
 for (unsigned char i=0, *pR=ResInterpret; i<nlinhas; i++, pR++)
   if (*pR != 255)
   {
      validLines[*pR] = true;
      _linhai[*pR] = linha[i];
      nRecogLinhas++;
   }

  float e; // utilizado pela macro intersecao
  if (nRecogLinhas > 4)
  {
    for (int i = 0; i < NPONTOS_MDL; i++)
      validPoints[i] = false;

	// Acha Pontos que interceptam a linha 1
    if (validLines[1])
	{
	  // Interseccao com linha 8
	  if (validLines[8])
	  {
        GeomPonto2D* p = &PontosReconhecidos[1];
		intersecao(_linhai[1], _linhai[8], p);
 	    validPoints[1] = true;
	  }
	  // Interseccao com linha 0
	  if (validLines[0])
	  {
        GeomPonto2D* p = &PontosReconhecidos[2];
	    intersecao(_linhai[1], _linhai[0], p);
		validPoints[2] = true;
	  }
	  // Interseccao com linha 2
	  if (validLines[2])
	  {
        GeomPonto2D* p = &PontosReconhecidos[3];
	    intersecao(_linhai[1], _linhai[2], p);
		validPoints[3] = true;
	  }
    }

	// Acha Pontos que interceptam a linha 5
    if (validLines[5])
	{
	  // Interseccao com linha 4
	  if (validLines[4])
	  {
        GeomPonto2D* p = &PontosReconhecidos[4];
	    intersecao(_linhai[5], _linhai[4], p);
	    validPoints[4] = true;
	  }
	  // Interseccao com linha 0
	  if (validLines[0])
	  {
        GeomPonto2D* p = &PontosReconhecidos[5];
	    intersecao(_linhai[5], _linhai[0], p);
		validPoints[5] = true;
	  }
	  // Interseccao com linha 6
	  if (validLines[6])
	  {
        GeomPonto2D* p = &PontosReconhecidos[6];
	    intersecao(_linhai[5], _linhai[6], p);
		validPoints[6] = true;
	  }
	}

	// Acha Pontos que interceptam a linha 7
    if (validLines[7])
	{
	  // Interseccao com linha 8
	  if (validLines[8])
	  {
        GeomPonto2D* p = &PontosReconhecidos[7];
	    intersecao(_linhai[7], _linhai[8], p);
		validPoints[7] = true;
	  }
	  // Interseccao com linha 4
	  if (validLines[4])
	  {
        GeomPonto2D* p = &PontosReconhecidos[8];
	    intersecao(_linhai[7], _linhai[4], p);
		validPoints[8] = true;
	  }
	  // Interseccao com linha 0
	  if (validLines[0])
	  {
        GeomPonto2D* p = &PontosReconhecidos[9];
	    intersecao(_linhai[7], _linhai[0], p);
		validPoints[9] = true;
	  }
	  // Interseccao com linha 6
	  if (validLines[6])
	  {
        GeomPonto2D* p = &PontosReconhecidos[10];
	    intersecao(_linhai[7], _linhai[6], p);
		validPoints[10] = true;
	  }
	}

	// Acha Pontos que interceptam a linha 3
    if (validLines[3])
	{
	  // Interseccao com linha 4
	  if (validLines[4])
	  {
        GeomPonto2D* p = &PontosReconhecidos[11];
	    intersecao(_linhai[3], _linhai[4], p);
		validPoints[11] = true;
	  }
	  // Interseccao com linha 0
	  if (validLines[0])
	  {
        GeomPonto2D* p = &PontosReconhecidos[12];
	    intersecao(_linhai[3], _linhai[0], p);
		validPoints[12] = true;
	  }
	  // Interseccao com linha 2
	  if (validLines[2])
	  {
        GeomPonto2D* p = &PontosReconhecidos[13];
	    intersecao(_linhai[3], _linhai[2], p);
		validPoints[13] = true;
	  }
	}
  }
  int nPontos = 0;
  for (int i = 0; i < NPONTOS_MDL; i++)
  {
    if (validPoints[i])
	{
      xw[nPontos] = (double) PontosMdl[i].x;
      yw[nPontos] = (double) PontosMdl[i].y;
      xp[nPontos] = (double) PontosReconhecidos[i].x;
	  yp[nPontos] = (double) PontosReconhecidos[i].y;
	  nPontos++;
	}
  }
  return nPontos;
}


static int ReconhecePontosModelo2Reconstruido()
{/*
  nRecogLinhas = 0;
  for (int i = 0; i < NLINHAS; i++)
    validLines[i] = false;

 TreeInterpLinha _linhai[NLINHAS];
 for (unsigned char i=0, *pR=ResInterpret; i<nlinhas; i++, pR++)
   if (*pR != 255)
   {
      validLines[*pR] = true;
      _linhai[*pR] = linha[i];
      nRecogLinhas++;
   }
*/
  float e; // utilizado pela macro intersecao
  if (1)//nRecogLinhas > 4)
  {
    for (int i = 0; i < NPONTOS_MDL; i++)
      validPoints[i] = false;

	// Acha Pontos que interceptam a linha 1
	if (lr1.valido)
	{
	  // Interseccao com linha 8
	  if (lr8.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[1];
		intersecao(lr1, lr8, p);
 	    validPoints[1] = true;
	  }
	  // Interseccao com linha 0
	  if (lr0.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[2];
	    intersecao(lr1, lr0, p);
		validPoints[2] = true;
	  }
	  // Interseccao com linha 2
	  if (lr2.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[3];
	    intersecao(lr1, lr2, p);
		validPoints[3] = true;
	  }
    }

	// Acha Pontos que interceptam a linha 5
    if (lr5.valido)
	{
	  // Interseccao com linha 4
	  if (lr4.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[4];
	    intersecao(lr5, lr4, p);
	    validPoints[4] = true;
	  }
	  // Interseccao com linha 0
	  if (lr0.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[5];
	    intersecao(lr5, lr0, p);
		validPoints[5] = true;
	  }
	  // Interseccao com linha 6
	  if (lr6.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[6];
	    intersecao(lr5, lr6, p);
		validPoints[6] = true;
	  }
	}

	// Acha Pontos que interceptam a linha 7
    if (lr7.valido)
	{
	  // Interseccao com linha 8
	  if (lr8.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[7];
	    intersecao(lr7, lr8, p);
		validPoints[7] = true;
	  }
	  // Interseccao com linha 4
	  if (lr4.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[8];
	    intersecao(lr7, lr4, p);
		validPoints[8] = true;
	  }
	  // Interseccao com linha 0
	  if (lr0.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[9];
	    intersecao(lr7, lr0, p);
		validPoints[9] = true;
	  }
	  // Interseccao com linha 6
	  if (lr6.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[10];
	    intersecao(lr7, lr6, p);
		validPoints[10] = true;
	  }
	}

	// Acha Pontos que interceptam a linha 3
    if (lr3.valido)
	{
	  // Interseccao com linha 4
	  if (lr4.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[11];
	    intersecao(lr3, lr4, p);
		validPoints[11] = true;
	  }
	  // Interseccao com linha 0
	  if (lr0.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[12];
	    intersecao(lr3, lr0, p);
		validPoints[12] = true;
	  }
	  // Interseccao com linha 2
	  if (lr2.valido)
	  {
        GeomPonto2D* p = &PontosReconhecidos[13];
	    intersecao(lr3, lr2, p);
		validPoints[13] = true;
	  }
	}
  }
  int nPontos = 0;
  for (int i = 0; i < NPONTOS_MDL; i++)
  {
    if (validPoints[i])
	{
      xw[nPontos] = (double) PontosMdl[i].x;
      yw[nPontos] = (double) PontosMdl[i].y;
      xp[nPontos] = (double) PontosReconhecidos[i].x;
	  yp[nPontos] = (double) PontosReconhecidos[i].y;
	  nPontos++;
	}
  }
  return nPontos;
}
/*
static int ReconhecePontosModelo2Reconstruido(GeomPonto2D *pts1, GeomPonto2D *pts2)
{
 float e;
 int npts = 0;
 GeomPonto2D *pp1 = pts1, *pp2 = pts2, *ppaux = &PONTOS[2];
 if (lr1.valido) {
  *pp1++ = *ppaux++;
  intersecao(lr0, lr1, pp2); pp2++;
  *pp1++ = *ppaux++;
  intersecao(lr1, lr2, pp2); pp2++;
  *pp1++ = *(ppaux+6);
  intersecao(lr1, lr5, pp2); pp2++;
  npts=3;
 } else ppaux+=2;
 if (lr3.valido) {
  *pp1++ = *ppaux++;
  intersecao(lr2, lr3, pp2); pp2++;
  *pp1++ = *ppaux++;
  intersecao(lr3, lr0, pp2); pp2++;
  *pp1++ = *(ppaux+7);
  intersecao(lr3, lr5, pp2); pp2++;
  npts+=3;
 } else ppaux+=2;
 if (lr4.valido) {
  *pp1++ = *ppaux++;
  intersecao(lr0, lr4, pp2); pp2++;
  *pp1++ = *ppaux++;
  intersecao(lr4, lr5, pp2); pp2++;
  *pp1++ = *(ppaux+3);
  intersecao(lr2, lr4, pp2); pp2++;
  npts+=3;
 } else ppaux+=2;
 if (lr6.valido) {
  *pp1++ = *ppaux++;
  intersecao(lr5, lr6, pp2); pp2++;
  *pp1++ = *ppaux++;
  intersecao(lr6, lr0, pp2); pp2++;
  *pp1++ = *(ppaux+2);
  intersecao(lr2, lr6, pp2);
  npts+=3;
 }
 return npts;
}*/

void initialialize(int width, int height)
{
  gWidth = width;
  gHeight = height;
}

void CarregaMeuModelo(char *fname)
{
  int i;
  FILE *fd = fopen(fname, "r");

  int numPontos;
    fscanf(fd, "%d", &numPontos);
  for (i=0; i<numPontos; i++)
    fscanf(fd, "%f %f", &(PontosMdl[i].x), &(PontosMdl[i].y));

  int numLinhas;
    fscanf(fd, "%d", &numLinhas);

  for (i=0; i<numLinhas; i++)
  {
    int v1, v2;
    fscanf(fd, "%d %d", &v1, &v2);
    LINHA[i].v1x = PontosMdl[v1].x,  LINHA[i].v1y = PontosMdl[v1].y,  LINHA[i].v2x = PontosMdl[v2].x, LINHA[i].v2y = PontosMdl[v2].y;
  }
  fclose(fd);
}

void desenhaLinhas()
{
  glColor3f(0.0f, 0.0f, 0.0f);
  glBegin(GL_LINES);
  for (int i=0; i<nlinhas; i++)
  {
    double v1x = linha[i].v1x/320.0 - 1;
    double v1y = linha[i].v1y/240.0;
    double v2x = linha[i].v2x/320.0 - 1;
    double v2y = linha[i].v2y/240.0;
    glVertex2d(v1x, v1y);
    glVertex2d(v2x, v2y);
  } 
  glEnd();
}

void desenhaLinhasReconhecidas()
{
/*
  glColor3f(1.0f, 0.0f, 0.0f);
  glBegin(GL_LINES);
  for (int i=0; i<nlinhas; i++)
  {
    if (ResInterpret[i]!=255 && ResInterpret[i] >=0 && ResInterpret[i] < NLINHAS)
    {
	  if (ResInterpret[i] == 0)
        glColor3f(0.0f, 1.0f, 0.0f);
	  else
        glColor3f(1.0f, 0.0f, 0.0f);
      double v1x = linha[i].v1x/320.0 - 1;
      double v1y = linha[i].v1y/240.0;
      double v2x = linha[i].v2x/320.0 - 1;
      double v2y = linha[i].v2y/240.0;
      glVertex2d(v1x, v1y);
      glVertex2d(v2x, v2y);
    }
  } 
  glEnd();
*/
  glColor3f(1.0f, 0.0f, 0.0f);
  glBegin(GL_LINES);
  for (int i=0; i<nlinhas; i++)
  {
	if (lr[i].valido)
	{
	  if (i == 0)
        glColor3f(0.0f, 1.0f, 0.0f);
	  else
        glColor3f(1.0f, 0.0f, 0.0f);
      double v1x = lr[i].v1x/320.0 - 1;
      double v1y = lr[i].v1y/240.0;
      double v2x = lr[i].v2x/320.0 - 1;
      double v2y = lr[i].v2y/240.0;
      glVertex2d(v1x, v1y);
      glVertex2d(v2x, v2y);
	}
  } 
  glEnd();

}

void desenhaPontos()
{
  glColor3f(0.0f, 1.0f, 0.0f);
  glPointSize(2.0f);
  glBegin(GL_POINTS);
  for (int i=0; i<NPONTOS_MDL; i++)
  {
	if (validPoints[i])
	{
      double vx = PontosReconhecidos[i].x/320.0 - 1;
      double vy = PontosReconhecidos[i].y/240.0;
      glVertex2d(vx, vy);
	}
  }
  glEnd();
}

void desenhaModelo()
{
  glColor3f(0.0f, 0.0f, 1.0f);
  glPointSize(2.0f);
  glBegin(GL_LINES);
  for (int i=0; i<NLINHAS; i++)
  {
	if (validPoints[i])
	{
	  double v1x = LINHA[i].v1x;
      double v1y = LINHA[i].v1y;
      double v2x = LINHA[i].v2x;
      double v2y = LINHA[i].v2y;
      glVertex2d(v1x, v1y);
      glVertex2d(v2x, v2y);
	}
  }
  glEnd();
}

int RestricaoModelo1(int nlinhas, TreeInterpLinha *linha, unsigned char *F)
{
 unsigned char *pF, *pF_max;
 int a=0, b=0, c=0, d=0, e=0, f=0;
 for (pF=F, pF_max=&F[nlinhas-1]; pF<=pF_max; pF++) {
  a += (*pF==0 || *pF==4);
  b += (*pF==3 || *pF==5);
  c += *pF==2;
  d += *pF==6;
  e += *pF==1;
  f += *pF==7;
 }
/* 
 if (!(a>1 && b>1))
   return 0;

 for (int i=0; i<nlinhas; i++)
 {
   if (F[i]!=255 && ResInterpret[i] >=0 && ResInterpret[i] < NLINHAS)
   {
     if (linha[i].compr2 < 20) return 0;
   }
 }
 return 1;
 */
 return (a>0 && b>0 && c>0 && d>0 && e>0 && f>0);
}

int recognizeInterpret(unsigned char* gData, int threshold, double* H)
{
  unsigned char* temp = new unsigned char[gHeight*gWidth];

#ifdef MODELO_1
  ConvolucaoLaplacianadaGaussianaCor(gData, temp, threshold);
#else
  for (int i = 0; i < gWidth; i++)
  {
    for (int j = 0; j <gHeight; j++)
    {
      temp[i*gHeight+j] = gData[3*(i*gHeight+j)];
   	  if (i == 0 || j == 0 || i == gWidth-1 || j == gHeight-1)
  	    temp[i*gHeight+j] = 0;
	}
  }
/*
	for (int i = 0; i < gWidth; i++)
	{
	  for (int j = 0; j <gHeight; j++)
	  {
		if (i == 0 || j == 0 || i == gWidth-1 || j == gHeight-1)
  	      gData[3*(i*gHeight+j)] = 0;
	  }
	}
*/
#endif

  nlinhas = edgedetect(temp, 10, 10, 20, linha);
  if (ResInterpret)
    delete ResInterpret;
  ResInterpret = new unsigned char [nlinhas];
  if (nlinhas > 4)
  {
#ifdef MODELO_1
    TreeInterpretation(NLINHAS, LINHA, nlinhas, linha, RestricaoModelo1, ResInterpret);
    nRecogPoints = ReconhecePontosModelo1();	  
#else
    TreeInterpretation(NLINHAS, LINHA, nlinhas, linha, NULL, ResInterpret);
    nRecogPoints = ReconhecePontosModelo2();	  
#endif
  }
  else
    return 0;

  if (!EncontraHomografia(H, nRecogPoints, xw, yw, xp, yp))
  {
    delete temp;
	return 0;
  }
  

  A00=H[0], A01=H[1], A02=H[2],
  A10=H[3], A11=H[4], A12=H[5],
  A20=H[6], A21=H[7], A22=H[8];

  if (!AjustaReconhecimento(temp,6))
  {
    delete temp;
    return 0;
  }

#ifdef MODELO_1
  nRecogPoints = ReconhecePontosModelo1Reconstruido();	  
#else
  nRecogPoints = ReconhecePontosModelo2Reconstruido();
#endif
  if (!EncontraHomografia(H, nRecogPoints, xw, yw, xp, yp))
  {
    delete temp;
	return 0;
  }
  float erromax = ErroMax(H, nRecogPoints, xw, yw, xp, yp);
  // float erromed = ErroMedio(Q3, npts, pts1, pts2);
  if (erromax > 20) return 0;

  delete temp;
  return 1;
}

int recognize(unsigned char* gData, int threshold, float lambda_extrap, float lambda_interp, double* H1, double* H2, double* H3)
{
  unsigned char* temp = new unsigned char[gHeight*gWidth];

#if 0//#ifdef MODELO_1
  ConvolucaoLaplacianadaGaussianaCor(gData, temp, threshold);
#else
  for (int i = 0; i < gWidth; i++)
  {
    for (int j = 0; j <gHeight; j++)
    {
      temp[i*gHeight+j] = gData[3*(i*gHeight+j)];
   	  if (i == 0 || j == 0 || i == gWidth-1 || j == gHeight-1)
  	    temp[i*gHeight+j] = 0;
    }
  }
/*
	for (int i = 0; i < gWidth; i++)
	{
	  for (int j = 0; j <gHeight; j++)
	  {
		if (i == 0 || j == 0 || i == gWidth-1 || j == gHeight-1)
  	      gData[3*(i*gHeight+j)] = 0;
	  }
	}
*/
#endif

  A00=H1[0], A01=H1[1], A02=H1[2],
  A10=H1[3], A11=H1[4], A12=H1[5],
  A20=H1[6], A21=H1[7], A22=H1[8];
  B00=H2[0], B01=H2[1], B02=H2[2],
  B10=H2[3], B11=H2[4], B12=H2[5],
  B20=H2[6], B21=H2[7], B22=H2[8];

  if (!AjustaReconhecimento(temp,6))
  {
    delete temp;
    return 0;
  }

#ifdef MODELO_1
  nRecogPoints = ReconhecePontosModelo1Reconstruido();	  
#else
  nRecogPoints = ReconhecePontosModelo2Reconstruido();
#endif
  for (int i=0; i<nRecogPoints; i++)
  {
    float w = A20*xw[i] + A21*yw[i] + A22;
    float p0x = (A00*xw[i] + A01*yw[i] + A02)/w;
    float p0y = (A10*xw[i] + A11*yw[i] + A12)/w;
    w = B20*xw[i] + B21*yw[i] + B22;
    float p1x = (B00*xw[i] + B01*yw[i] + B02)/w;
    float p1y = (B10*xw[i] + B11*yw[i] + B12)/w;
    float x = lambda_extrap*(2*p1x - p0x) + (1-lambda_extrap)*p1x;
    float y = lambda_extrap*(2*p1y - p0y) + (1-lambda_extrap)*p1y;
    xp[i] = lambda_interp*x + (1-lambda_interp)*xp[i];
    yp[i] = lambda_interp*y + (1-lambda_interp)*yp[i];
  }
  if (!EncontraHomografia(H3, nRecogPoints, xw, yw, xp, yp))
  {
    delete temp;
	return 0;
  }
  float erromax = ErroMax(H3, nRecogPoints, xw, yw, xp, yp);
  // float erromed = ErroMedio(Q3, npts, pts1, pts2);
  if (erromax > 20) return 0;
  
  delete temp;
  return 1;
}

int recognize(unsigned char* gData, int threshold, double* H)
{
  unsigned char* temp = new unsigned char[gHeight*gWidth];

#if 0//#ifdef MODELO_1
  ConvolucaoLaplacianadaGaussianaCor(gData, temp, threshold);
#else
  for (int i = 0; i < gWidth; i++)
  {
    for (int j = 0; j <gHeight; j++)
    {
      temp[i*gHeight+j] = gData[3*(i*gHeight+j)];
   	  if (i == 0 || j == 0 || i == gWidth-1 || j == gHeight-1)
  	    temp[i*gHeight+j] = 0;
    }
  }
/*
	for (int i = 0; i < gWidth; i++)
	{
	  for (int j = 0; j <gHeight; j++)
	  {
		if (i == 0 || j == 0 || i == gWidth-1 || j == gHeight-1)
  	      gData[3*(i*gHeight+j)] = 0;
	  }
	}
*/
#endif

  A00=H[0], A01=H[1], A02=H[2],
  A10=H[3], A11=H[4], A12=H[5],
  A20=H[6], A21=H[7], A22=H[8];

  if (!AjustaReconhecimento(temp,6))
  {
    delete temp;
    return 0;
  }

#ifdef MODELO_1
  nRecogPoints = ReconhecePontosModelo1Reconstruido();	  
#else
  nRecogPoints = ReconhecePontosModelo2Reconstruido();
#endif
  if (!EncontraHomografia(H, nRecogPoints, xw, yw, xp, yp))
  {
    delete temp;
	return 0;
  }
  float erromax = ErroMax(H, nRecogPoints, xw, yw, xp, yp);
  // float erromed = ErroMedio(Q3, npts, pts1, pts2);
  if (erromax > 20) return 0;
  
  delete temp;
  return 1;
}


int getNumberRecognizedLines()
{
  return nRecogLinhas;
}

bool CalibrateCamera(double* modelview, double* projection)
{
  double focal_length;
  double Translation[3], Rotation[9];

  double xf[NPONTOS_MDL];
  double yf[NPONTOS_MDL];

  for (int i = 0; i < nRecogPoints; i++)
  {
    xf[i] = (double) xp[i]-gWidth/2;
    yf[i] = (double) yp[i]-gHeight/2;
  }

  tsaiCalibration2D(nRecogPoints, xw, yw, xf, yf, &focal_length, Translation, Rotation);
  tsaiOGLModelViewMatrix(Translation, Rotation, modelview);
  tsaiOGLProjectionMatrix(focal_length, NEARPLANE, FARPLANE, gWidth, gHeight, projection);
  return true;
}