#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <memory.h>
#include "jv.h"

#ifndef SWAP
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
#endif

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

#ifndef ABS
#define ABS(a) (((a)<0)?-(a):(a))
#endif

#ifndef MIN
#define MIN(a,b) (((a)<(b))? (a): (b))
#endif

#ifndef MAX
#define MAX(a,b) (((a)>(b))? (a): (b))
#endif

#define TOL 1e-10

#define MAXLINHAS 100

#define FATORCOLINEAR 0.997f
static float s_fatorcolinear = FATORCOLINEAR;

static float s_tol[MAXLINHAS];
static float s_aux[MAXLINHAS];
static jvPoint2D s_lmin[MAXLINHAS];
static jvPoint2D s_lmax[MAXLINHAS];

/* Vide tese "Acompanhamento de Cenas com Calibração Automática de Câmeras". */
#define ETA 2.0f /* fator de distorção perspectiva dada na direção vertical da imagem. */
#define RO 0.65f /* tolerância de paralelismo. */

static float s_eta = ETA;
static float s_ro = RO;
static int s_n_lines_img;
static jvLine *s_lines_img;

static int s_width, s_height;
static int s_dw, s_dh;

static int *s_gaussiana = NULL;

static int     s_n_lines_mdl;
static jvLine *s_lines_mdl = NULL;

static size_t s_p2d = sizeof(jvPoint2D);

static int s_npontos[MAXLINHAS];
static jvPoint2D **s_pontos = NULL;
static unsigned char **s_pesos = NULL;

static int  s_lcel, s_acel;
static int *s_ID = NULL;
static unsigned char *s_id = NULL;

static unsigned char s_F[MAXLINHAS+1];

static void *s_userdata;

static PFNINTERPPROC s_interp_function;

int  jvInit(int w, int h, int dw, int dh, char* model_file_name)
{
 int i;
 FILE *fd;

 jvClose();

 fd = fopen(model_file_name, "r");
 if (!fd) return 0;
 if (fscanf(fd, "%d", &s_n_lines_mdl) != 1) return 0;
 s_lines_mdl = (jvLine *)malloc(s_n_lines_mdl*sizeof(jvLine));
 for (i=0; i<s_n_lines_mdl; i++)
  if (fscanf(fd, "%f %f %f %f", &(s_lines_mdl[i].v1x), &(s_lines_mdl[i].v1y), &(s_lines_mdl[i].v2x), &(s_lines_mdl[i].v2y)) != 4) return 0;
 fclose(fd);

 s_lcel = w/dw, s_acel = h/dh;

 s_pontos = (jvPoint2D **)malloc(MAXLINHAS*sizeof(jvPoint2D *));
 s_pesos = (unsigned char **)malloc(MAXLINHAS*sizeof(unsigned char *));
 for (i=0; i<MAXLINHAS; i++) {
  s_pontos[i] = (jvPoint2D *)malloc(2*MAX(w*dh,dw*h)*sizeof(jvPoint2D));
  s_pesos[i] = (unsigned char *)malloc(2*MAX(w*dh,dw*h)*sizeof(unsigned char));
 }

 s_ID = (int *)malloc(s_lcel*s_acel*sizeof(int));
 s_id = (unsigned char *)malloc(s_lcel*s_acel*sizeof(unsigned char));

 s_width = w, s_height = h;
 s_dw = dw, s_dh = dh;
 s_gaussiana = (int *)malloc(w*h*sizeof(int));

 return 1;
}

void jvLoGFilter(unsigned char *img_in, int threshold, unsigned char *img_out)
{
 int i, j, aux;

 s_gaussiana[0] = 4*img_in[(0+1)*s_width + (0)] + 4*img_in[(0+1)*s_width + (0+1)] +
                  4*img_in[(0  )*s_width + (0)] + 4*img_in[(0  )*s_width + (0+1)];
 for (i=1; i<s_width-1; i++)
  s_gaussiana[i] = 2*img_in[(0+1)*s_width + (i-1)] + 4*img_in[(0+1)*s_width + (i)] + 2*img_in[(0+1)*s_width + (i+1)] +
                   2*img_in[(0  )*s_width + (i-1)] + 4*img_in[(0  )*s_width + (i)] + 2*img_in[(0  )*s_width + (i+1)];
  s_gaussiana[i] = (img_in[i-1+s_width] + (img_in[i+s_width] + img_in[i]) * 2 + img_in[i+1+s_width] + img_in[i-1] + img_in[i+1]) * 2;
 s_gaussiana[s_width-1] = 4*img_in[(0+1)*s_width + (s_width-2)] + 4*img_in[(0+1)*s_width + (s_width-1)] +
                          4*img_in[(0  )*s_width + (s_width-2)] + 4*img_in[(0  )*s_width + (s_width-1)];

 for (j=1; j<s_height-1; j++) {
  s_gaussiana[j*s_width] = 2*img_in[(j+1)*s_width + (i)] + 2*img_in[(j+1)*s_width + (i+1)] +
                           4*img_in[(j  )*s_width + (i)] + 4*img_in[(j  )*s_width + (i+1)] +
                           2*img_in[(j-1)*s_width + (i)] + 2*img_in[(j-1)*s_width + (i+1)];
  for (i=1; i<s_width-1; i++)
   s_gaussiana[j*s_width + i] =   img_in[(j+1)*s_width + (i-1)] + 2*img_in[(j+1)*s_width + (i)] +   img_in[(j+1)*s_width + (i+1)] +
                                2*img_in[(j  )*s_width + (i-1)] + 4*img_in[(j  )*s_width + (i)] + 2*img_in[(j  )*s_width + (i+1)] +
                                  img_in[(j-1)*s_width + (i-1)] + 2*img_in[(j-1)*s_width + (i)] +   img_in[(j-1)*s_width + (i+1)];
  s_gaussiana[j*s_width + i] = 2*img_in[(j+1)*s_width + (i-1)] + 2*img_in[(j+1)*s_width + (i)] +
                               4*img_in[(j  )*s_width + (i-1)] + 4*img_in[(j  )*s_width + (i)] +
                               2*img_in[(j-1)*s_width + (i-1)] + 2*img_in[(j-1)*s_width + (i)];
 }

 s_gaussiana[(s_height-1)*s_width + 0] = 4*img_in[(s_height-1)*s_width + (0)] + 4*img_in[(s_height-1)*s_width + (0+1)] +
                                         4*img_in[(s_height-2)*s_width + (0)] + 4*img_in[(s_height-2)*s_width + (0+1)];
 for (i=1; i<s_width-1; i++)
  s_gaussiana[(s_height-1)*s_width + i] = 2*img_in[(s_height-1)*s_width + (i-1)] + 4*img_in[(s_height-1)*s_width + (i)] + 2*img_in[(s_height-1)*s_width + (i+1)] +
                                          2*img_in[(s_height-2)*s_width + (i-1)] + 4*img_in[(s_height-2)*s_width + (i)] + 2*img_in[(s_height-2)*s_width + (i+1)];
 s_gaussiana[(s_height-1)*s_width + (s_width-1)] = 4*img_in[(s_height-1)*s_width + (s_width-2)] + 4*img_in[(s_height-1)*s_width + (s_width-1)] +
                                                   4*img_in[(s_height-2)*s_width + (s_width-2)] + 4*img_in[(s_height-2)*s_width + (s_width-1)];

 memset(img_out, 0, s_width*s_height);
 for (j=1; j<s_height-1; j++)
  for (i=1; i<s_width-1; i++) {
   aux = 0                                 -   s_gaussiana[(j+1)*s_width + (i)] + 0                                +
         -s_gaussiana[(j)*s_width + (i-1)] + 4*s_gaussiana[(j)*s_width + (i)]   - s_gaussiana[(j)*s_width + (i+1)] +
         0                                 -   s_gaussiana[(j-1)*s_width + (i)] + 0;
   if (aux < 16*threshold)
    img_out[j*s_width + i] = 0;
   else
  //  img_out[j*s_width + i] = (unsigned char)(aux / 16.0);
      img_out[j*s_width + i] = 16* ( (unsigned char) sqrt( (double)aux / 16.0));
  }
}

#define EPSILON		1.0E-8

static int direcaoimagem(unsigned char *iin, int x1, int y1, float fator, float *vx, float *vy)
{
 int xmaux = 0, ymaux = 0, cont=0, i, j, cont2;
 float xm, ym;
 float a, b, c;
 float amc, delta;

 for (j=y1; j<y1+s_dh; j++)
  for (i=x1; i<x1+s_dw; i++)
   if (iin[j*s_width + i]) {
    xmaux += i*iin[j*s_width + i];
    ymaux += j*iin[j*s_width + i];
    cont += iin[j*s_width + i];
   }
 if (cont<5) return 0;

 xm = xmaux/((float)cont), ym = ymaux/((float)cont);

 a = 0.0f, b = 0.0f, c = 0.0f;
 for (j=y1, cont2=0; cont2<cont; j++)
  for (i=x1; i<x1+s_dw; i++)
   if (iin[j*s_width + i]) {
    a += (i-xm)*(i-xm)*iin[j*s_width + i];
    b += (i-xm)*(j-ym)*iin[j*s_width + i];
    c += (j-xm)*(j-ym)*iin[j*s_width + i];
    cont2 += iin[j*s_width + i];
    if (cont2 == cont) break;
   }

 amc = a-c, delta = amc*amc + 4*b*b;
 if (delta < EPSILON) return 0;
 delta = (float)sqrt(delta);
 if ((a+c+delta) < fator*(a+c-delta)) return 0;
 if (a < EPSILON) {
  *vx = 0.0f, *vy = 1.0f;
  return 1;
 } else if (c < EPSILON) {
  *vx = 1.0f, *vy = 0.0f;
  return 1;
 } else if (ABS(b) < EPSILON) {
  if (amc > 0) {
   *vx = 1.0f, *vy = 0.0f;
   return 1;
  } else {
   *vx = 0.0f, *vy = 1.0f;
   return 1;
  }
 }
 *vx = (float)((amc+delta)/(2.0*b)),
 *vy = (float)(1.0/sqrt((*vx)*(*vx) + 1.0)),
 *vx *= *vy;
 return 1;
}

static int direcaoimagem2(unsigned char *iin, int x1, int y1, int x3, int y3, float fator, float *vx, float *vy)
{
 int i, j, xmaux = 0, ymaux = 0, cont=0, cont2=0;
 float xm, ym;
 float a, b, c;
 float amc, delta;

 for (j=y1; j<y1+s_dh; j++)
  for (i=x1; i<x1+s_dw; i++)
   if (iin[j*s_width + i]) {
    xmaux += i*iin[j*s_width + i];
    ymaux += j*iin[j*s_width + i];
    cont += iin[j*s_width + i];
   }
 for (j=y3; j<y3+s_dh; j++)
  for (i=x3; i<x3+s_dw; i++)
   if (iin[j*s_width + i]) {
    xmaux += i*iin[j*s_width + i];
    ymaux += j*iin[j*s_width + i];
    cont += iin[j*s_width + i];
   }

 xm = xmaux/((float)cont), ym = ymaux/((float)cont);

 a = 0.0f, b = 0.0f, c = 0.0f;
 for (j=y1; j<y1+s_dh; j++)
  for (i=x1; i<x1+s_dw; i++)
   if (iin[j*s_width + i]) {
    a += (i-xm)*(i-xm)*iin[j*s_width + i];
    b += (i-xm)*(j-ym)*iin[j*s_width + i];
    c += (j-ym)*(j-ym)*iin[j*s_width + i];
    cont2 += iin[j*s_width + i];
   }

 for (j=y3; j<y3+s_dh; j++)
  for (i=x3; i<x3+s_dw; i++)
   if (iin[j*s_width + i]) {
    a += (i-xm)*(i-xm)*iin[j*s_width + i];
    b += (i-xm)*(j-ym)*iin[j*s_width + i];
    c += (j-ym)*(j-ym)*iin[j*s_width + i];
    cont2 += iin[j*s_width + i];
   }

 amc = a-c, delta = amc*amc + 4*b*b;
 if (delta < EPSILON) return 0;
 delta = (float)sqrt(delta);
 if ((a+c+delta) < fator*(a+c-delta)) return 0;
 if (a < EPSILON) {
  *vx = 0.0f, *vy = 1.0f;
  return 1;
 } else if (c < EPSILON) {
  *vx = 1.0f, *vy = 0.0f;
  return 1;
 } else if (ABS(b) < EPSILON) {
  if (amc > 0) {
   *vx = 1.0f, *vy = 0.0f;
  } else {
   *vx = 0.0f, *vy = 1.0f;
  }
  return 1;
 }
 *vx = (float)((amc+delta)/(2.0*b)),
 *vy = (float)(1.0/sqrt((*vx)*(*vx) + 1.0)),
 *vx *= *vy;
 return 1;
}

static int colinear(jvPoint2D *p1, jvPoint2D *pm, jvLine *lines)
{
 float x2 = lines->v2x - lines->v1x,
       y2 = lines->v2y - lines->v1y,
       pi = p1->x*x2+p1->y*y2;
 if (pi*pi < s_fatorcolinear*s_fatorcolinear*(x2*x2 + y2*y2)) return 0;

 x2 = lines->v1x - pm->x,
 y2 = lines->v1y - pm->y,
 pi = p1->x*x2+p1->y*y2;
 if (pi*pi < s_fatorcolinear*s_fatorcolinear*(x2*x2 + y2*y2)) return 0;

 x2 = lines->v2x - pm->x,
 y2 = lines->v2y - pm->y,
 pi = p1->x*x2+p1->y*y2;
 return 1-(pi*pi < s_fatorcolinear*s_fatorcolinear*(x2*x2 + y2*y2));
}

static void ExtraiPontos(unsigned char *imagem, int x1, int y1, jvPoint2D *pontos, int *npontos, unsigned char *pesos)
{
 int i, j, k;
 int np = *npontos;

 for (k=0, j=y1; j<y1+s_dh; j++)
  for (i=x1; i<x1+s_dw; i++)
   if (imagem[j*s_width + i]) {
    pesos[np+k] = imagem[j*s_width + i];
    pontos[np+k].x = (float)i; pontos[np+k].y = (float)j;
    k++;
   }
 *npontos = np+k;
}

static void ExtraiLinha(jvPoint2D *pontos, unsigned char *pesos, int npontos, jvLine *l)
{
 float mx, my, Mx, My;
 int npaux = 0;
 float sxy=0, sxx=0, sy=0, sx=0, ptxps;
 float a, aux, b;
 jvPoint2D p[4];
 int i, j;

 mx = Mx = pontos[0].x;
 my = My = pontos[0].y;
 for (i=1; i<npontos; i++) {
  if (pontos[i].x < mx) mx = pontos[i].x;
  else if (pontos[i].x > Mx) Mx = pontos[i].x;
  if (pontos[i].y < my) my = pontos[i].y;
  else if (pontos[i].y > My) My = pontos[i].y;
}

 if (Mx-mx < 2) {
  for (i=0; i<npontos; i++)
   sx += pontos[i].x;
  l->v1x = sx/((float)npontos), l->v1y = my,
  l->v2x = l->v1x,              l->v2y = My;
  return;
 }

 if (My-my < 2) {
  for (i=0; i<npontos; i++)
   sy += pontos[i].y;
  l->v1x = mx, l->v1y = sy/((float)npontos),
  l->v2x = Mx, l->v2y = l->v1y;
  return;
 }

 for (i=0; i<npontos; i++){
  ptxps = pontos[i].x * pesos[i],
  sx += ptxps,
  sy += pontos[i].y * pesos[i],
  sxy += pontos[i].y * ptxps,
  sxx += pontos[i].x * ptxps,
  npaux += pesos[i];
 }

 if ((aux=npaux*sxx-sx*sx)==0 || (a=npaux*sxy-sx*sy)==0) {
  l->v1x = mx, l->v1y = my,
  l->v2x = Mx, l->v2y = My;
  return;
 }
 a /= aux;
 b = (sy - sx*a)/npaux;

 p[0].x = mx,       p[0].y = a*mx + b,
 p[1].x = Mx,       p[1].y = a*Mx + b,
 p[2].x = (my-b)/a, p[2].y = my,
 p[3].x = (My-b)/a, p[3].y = My;

 for (i=0; i<3; i++)
  for (j=i+1; j<4; j++)
   if (p[j].x < p[i].x) {
    jvPoint2D paux = p[i];
    p[i] = p[j];
    p[j] = paux;
   }

 l->v1x = p[1].x, l->v1y = p[1].y,
 l->v2x = p[2].x, l->v2y = p[2].y;
}

static int UniaoLinhas (unsigned char *iin, int idmax, jvLine *lines)
{
 int i, j;
 int n_lines_img = 0;
 int *IDINI;
 int id;
 jvLine linha2[MAXLINHAS];
 int nlinhas2;
 jvPoint2D d;
 int flag2 = 0;

 IDINI = &s_ID[s_width/s_dw];
 for (id=0; id<idmax; id++)
  if (s_id[id] > 1) {
   int cont=0;
   int *pID = IDINI;
   s_npontos[n_lines_img] = 0;
   for (j=s_dh; cont<s_id[id]; j+=s_dh, pID++)
    for (i=s_dw, pID++; i<s_width-s_dw; i+=s_dw)
     if (*pID++ == id) {
      ExtraiPontos(iin, i, j, s_pontos[n_lines_img], &s_npontos[n_lines_img], s_pesos[n_lines_img]);
      cont++;
      if (cont == s_id[id]) break;
     }
   ExtraiLinha(s_pontos[n_lines_img], s_pesos[n_lines_img], s_npontos[n_lines_img], &lines[n_lines_img]);
   n_lines_img++;
  }

 if (!n_lines_img) return 0;

 do {
  flag2 = 0;
  for (i=0; i<n_lines_img-1; i++)
   if (s_npontos[i]) {
    jvPoint2D p;
    float normal1;
    int flag = 0;
    d.x = lines[i].v2x - lines[i].v1x;
    d.y = lines[i].v2y - lines[i].v1y;
    normal1 = (float)sqrt(d.x*d.x + d.y*d.y);
    d.x /= normal1;
    d.y /= normal1;
    p.x = (lines[i].v1x + lines[i].v2x)*0.5f;
    p.y = (lines[i].v1y + lines[i].v2y)*0.5f;
    for (j=i+1; j<n_lines_img; j++)
     if (s_npontos[j]!=0 && colinear(&d, &p, &lines[j])) {
      flag = 1;
      memcpy(&(s_pontos[i])[s_npontos[i]], s_pontos[j], s_npontos[j]*s_p2d);
      memcpy(&(s_pesos[i])[s_npontos[i]], s_pesos[j], s_npontos[j]);
      s_npontos[i] += s_npontos[j];
      s_npontos[j] = 0;
     }
    if (flag) {
     ExtraiLinha(s_pontos[i], s_pesos[i], s_npontos[i], &lines[i]);
     flag2 = 1;
    }
   }
 } while (flag2 == 1);

 for (i=0, nlinhas2=0; i<n_lines_img; i++) {
  d.x = lines[i].v2x - lines[i].v1x;
  d.y = lines[i].v2y - lines[i].v1y;
  if (s_npontos[i] && (d.x*d.x + d.y*d.y > (s_dh*s_dh)+(s_dw*s_dw))) {
   linha2[nlinhas2] = lines[i];
   nlinhas2++;
  }
 }

 n_lines_img = nlinhas2;
 memcpy(lines, linha2, n_lines_img*sizeof(jvLine));

 return n_lines_img;
}

int jvEdgeDetect(unsigned char *img, float limit_ratio, jvLine *img_lines, float colinear_factor)
{
 float vx, vy;
 int cont, pos, i, j;

 s_fatorcolinear = FATORCOLINEAR * colinear_factor;

 memset(s_id, 0, s_lcel*s_acel);
 for (pos=0; pos<s_lcel; s_ID[pos++]=0);
 for (j=s_dh, cont=1; j<s_height-s_dh; j+=s_dh) {
  s_ID[pos++] = 0;
  for (i=s_dw; i<s_width-s_dw; i+=s_dw, pos++)
   if (direcaoimagem(img, i, j, limit_ratio, &vx, &vy)) {
    float vx2, vy2;
    if      (vx >= 0 && 
             s_ID[pos - 1 - s_lcel] != 0 &&
             direcaoimagem2(img, i, j, i-s_dw, j-s_dh, limit_ratio, &vx2, &vy2) && 
             (vx*vx2 + vy*vy2) > s_fatorcolinear)
     s_ID[pos] = s_ID[pos - 1 - s_lcel];
    else if (s_ID[pos - s_lcel] != 0 &&
             direcaoimagem2(img, i, j, i, j-s_dh, limit_ratio, &vx2, &vy2) && 
             (vx*vx2 + vy*vy2) > s_fatorcolinear)
     s_ID[pos] = s_ID[pos - s_lcel];
    else if (vx <= 0 && 
             s_ID[pos + 1 - s_lcel] != 0 &&
             direcaoimagem2(img, i, j, i+s_dw, j-s_dh, limit_ratio, &vx2, &vy2) && 
             (vx*vx2 + vy*vy2) > s_fatorcolinear)
     s_ID[pos] = s_ID[pos + 1 - s_lcel];
    else if (s_ID[pos - 1] != 0 &&
             direcaoimagem2(img, i, j, i-s_dw, j, limit_ratio, &vx2, &vy2) && 
             (vx*vx2 + vy*vy2) > s_fatorcolinear)
     s_ID[pos] = s_ID[pos - 1];
    else s_ID[pos] = cont++;
    s_id[s_ID[pos]]++;
   } else s_ID[pos] = 0;
   s_ID[pos++] = 0;
 }

 if (cont < 5) return 0;
 return UniaoLinhas(img, cont, img_lines);
}

static int sinall(jvLine l1, jvLine l2)
{
 jvPoint2D d;
 float S1, S2, V1, aux;
 d.x = l2.v2x - l2.v1x;
 d.y = l2.v2y - l2.v1y;
 V1 = 5*(d.x*d.x + d.y*d.y);
 aux = l2.v1x*l2.v2y - l2.v2x*l2.v1y;
 S1 = aux + l1.v1y*d.x - l1.v1x*d.y;
 if (SQR(S1) < V1) S1 = 0.0;
 S2 = aux + l1.v2y*d.x - l1.v2x*d.y;
 if (SQR(S2) < V1) S2 = 0.0;

 if ((S1==0 && S2==0) || S1*S2<0) return 0;
 if (S1>=0 && S2>=0) return 1;
 return -1;
}

static int sinalL(jvLine l1, jvLine l2)
{
 jvPoint2D d2;
 float S1, S2, aux;
 d2.x = l2.v2x - l2.v1x;
 d2.y = l2.v2y - l2.v1y;
 aux = l2.v1x*l2.v2y - l2.v2x*l2.v1y;
 S1 = aux + l1.v1y*d2.x - l1.v1x*d2.y;
 if (SQR(S1) < 1e-7) S1 = 0.0;
 S2 = aux + l1.v2y*d2.x - l1.v2x*d2.y;
 if (SQR(S2) < 1e-7) S2 = 0.0;

 if ((S1==0 && S2==0) || S1*S2<0) return 0;
 if (S1>=0 && S2>=0) return 1;
 return -1;
}

static int dirl(jvLine l1, jvLine l2)
{
 float b, d, e;
 jvPoint2D d1, d2;
 d1.x = l1.v2x - l1.v1x;
 d1.y = l1.v2y - l1.v1y;
 d2.x = l2.v2x - l2.v1x;
 d2.y = l2.v2y - l2.v1y;
 b = s_eta*d1.y;
 d = s_eta*d2.y;
 e = d1.x*d2.x + b*d;
 return e*e > s_ro*(d1.x*d1.x+b*b)*(d2.x*d2.x+d*d);
}

#define dirL(l1, l2) ((ABS(l1.v2x - l1.v1x)<1e-5 && ABS(l2.v2x - l2.v1x)<1e-5) || (ABS(l1.v2y - l1.v1y)<1e-5 && ABS(l2.v2y - l2.v1y)<1e-5))

static int Restricoes(jvLine *lines_img, int f, unsigned char *F)
{
 int i, j, k;

 if (F[f] == 255) return 1;

 for (i=0; i<f; i++)
  if (F[i] == F[f]) return 0; // Impede a repetição

 // Dois segmentos paralelos (ou não) entre si tem seus correspondentes paralelos (ou não).
 for (i=0; i<f; i++)
  if ((F[i] != 255) && (dirl(lines_img[i], lines_img[f]) != dirL(s_lines_mdl[F[i]], s_lines_mdl[F[f]])))
   return 0;

 for (i=0; i<=f; i++) 
  if (F[i]!=255)
   for (j=0; j<=f; j++)
    if (j!=i && F[j]!=255) {
     int a1 = sinalL(s_lines_mdl[F[j]], s_lines_mdl[F[i]]);
     if (a1 != 0) {
      int a3 = sinall(lines_img[j], lines_img[i]);
      if (a3==0) return 0;
      for (k=0; k<=f; k++)
       if (k!=i && k!=j && F[k]!=255) {
        int a2 = sinalL(s_lines_mdl[F[k]], s_lines_mdl[F[i]]);
        int a4 = sinall(lines_img[k], lines_img[i]);
        if ((a4==0 && a2!=0) || (a2!=0 && (a1*a2 != a3*a4))) return 0;
       }
     }
    }
 return 1;
}

static int BuscaEmArvore(int f, unsigned char *F)
{
 if (f == s_n_lines_img)
  return s_interp_function(s_userdata, s_n_lines_img, s_lines_img, s_n_lines_mdl, s_lines_mdl, F);
 else {
  unsigned char NF;
  F[f] = 255;
  if (Restricoes(s_lines_img, f, F))
   if (BuscaEmArvore(f+1, F)) return 1;
  for (NF=0; NF<s_n_lines_mdl; NF++) {
   F[f] = NF;
   if (Restricoes(s_lines_img, f, F))
    if (BuscaEmArvore(f+1, F)) return 1;
  }
 }
 return 0;
}

void jvInterpret(void *userdata, int n_lines_img, jvLine *lines_img, float eta, float ro, PFNINTERPPROC interp_function)
{
 unsigned char NF;

 if (n_lines_img <= 0) return;

 s_userdata = userdata;
 s_n_lines_img = n_lines_img;
 s_lines_img = lines_img;
 s_eta = ETA*eta;
 s_ro = RO*ro;
 s_interp_function = interp_function;

 s_F[0] = 255;
 if (BuscaEmArvore(1, s_F)) return;
 for (NF=0; NF<s_n_lines_mdl; NF++) {
  s_F[0] = NF;
  if (BuscaEmArvore(1, s_F)) return;
 }
}

void jvAdjust(unsigned char *img, int n_lines_img, jvLine *lines_img, int tol)
{
   int i, j, k;
   float minx, maxx, miny, maxy ;
   int tol2 ;
   int xm, ym, xM, yM ;
   jvPoint2D d;

   if (n_lines_img <= 0) return;

   for( i=0; i<n_lines_img; i++ )
      s_npontos[i] = 0 ;

   minx = (float)s_width, miny = (float)s_height, maxx = 0.0f, maxy = 0.0f ;

   tol2 = tol*tol ;

   for( i=0; i<n_lines_img; i++ )
   {
      if( lines_img[i].valid )
      {
         if (lines_img[i].v1x < lines_img[i].v2x) 
         {
            if ((s_lmin[i].x = lines_img[i].v1x-tol) < minx) minx = s_lmin[i].x;
            if ((s_lmax[i].x = lines_img[i].v2x+tol) > maxx) maxx = s_lmax[i].x;
         }
         else
         {
            if ((s_lmin[i].x = lines_img[i].v2x-tol) < minx) minx = s_lmin[i].x;
            if ((s_lmax[i].x = lines_img[i].v1x+tol) > maxx) maxx = s_lmax[i].x;
         }
         if (lines_img[i].v1y < lines_img[i].v2y)
         {
            if ((s_lmin[i].y = lines_img[i].v1y-tol) < miny) miny = s_lmin[i].y;
            if ((s_lmax[i].y = lines_img[i].v2y+tol) > maxy) maxy = s_lmax[i].y;
         }
         else
         {
            if ((s_lmin[i].y = lines_img[i].v2y-tol) < miny) miny = s_lmin[i].y;
            if ((s_lmax[i].y = lines_img[i].v1y+tol) > maxy) maxy = s_lmax[i].y;
         }
         d.x = lines_img[i].v2x - lines_img[i].v1x,
         d.y = lines_img[i].v2y - lines_img[i].v1y;
         s_tol[i] = (d.x*d.x + d.y*d.y)*tol2;
         s_aux[i] = d.x*(lines_img[i].v1y+lines_img[i].v2y);
      }
   }

   xm = (int)((minx < 1)? 1 : minx);
   ym = (int)((miny < 1)? 1 : miny);
   xM = (int)((maxx > s_width-1)? s_width-1 : maxx);
   yM = (int)((maxy > s_height-1)? s_height-1 : maxy);
   for (j=ym; j<=yM; j++)
   {
      for (i=xm; i<=xM; i++)
      {
         if (img[j*s_width + i])
         {
            float area;
            int nl=-1;
            for (k=0; k<n_lines_img && nl!=-2; k++)
            {
               if( lines_img[k].valid )
               {
                  if (i>s_lmin[k].x && i<s_lmax[k].x && j>s_lmin[k].y && j<s_lmax[k].y)
                  {
                     area=((lines_img[k].v2x - lines_img[k].v1x)-(i - lines_img[k].v1x))*(lines_img[k].v2y+j) + (i - lines_img[k].v1x)*(j + lines_img[k].v1y) - s_aux[k];
                     area=SQR(area);
                     if (area <= s_tol[k])
                        nl = (nl==-1)?k:-2;
                  }
               }
            }
            if (nl > -1)
            {
               s_pontos[nl][s_npontos[nl]].x = (float)i;
               s_pontos[nl][s_npontos[nl]].y = (float)j;
               s_pesos[nl][s_npontos[nl]] = img[j*s_width + i];
               s_npontos[nl]++;
            }
         }
      }
   }

   for (k=0; k<n_lines_img; k++) 
   {
      if( lines_img[k].valid )
      {
         if (s_npontos[k] > 1)
         {
            ExtraiLinha(s_pontos[k], s_pesos[k], s_npontos[k], &lines_img[k]);
            lines_img[k].valid = 1;
         }
         else
            lines_img[k].valid = 0;
      }
   }
}

static int Gauss (int n,             // [in] Número de equações      
                  float a[10][10],   // [in] Matriz dos coeficientes 
                  float b[10])       // [in] Vetor das constantes    
                                     // [out] Vetor solução          
                                     // Valor de retorno : 0 => sem solução ou infinitas soluções
                                     //                    1 => solução única
{
 int    i, j, k;
 int    imax;             // Linha pivot
 float amax, rmax, temp;  // Auxiliares

 for (j=0; j<n-1; j++) {  // Loop nas colunas de a
  rmax = 0.0f;
  imax = j;
  for (i=j; i<n; i++) {   // Loop para determinar maior relacao a[i][j]/a[i][k]
   amax = 0.0f;
   for (k=j; k<n; k++)    // Loop para determinar maior elemento da linha i
   if (ABS(a[i][k]) > amax)
    amax = ABS(a[i][k]);
   if (amax < TOL)        // Verifica se os elementos da linha são nulos
    return 0;             // Finaliza subrotina pois o sistema não tem solução
   else if ((ABS(a[i][j]) > rmax*amax) && (ABS(a[i][j]) >= TOL)) { // Testa relação da linha corrente
    rmax = ABS(a[i][j]) / amax;
    imax = i;             // Encontra linha de maior relação - linha pivot
   }
  }
  if (ABS(a[imax][j])<TOL) {         // Verifica se o pivot é nulo
   for (k=imax+1; k<n; k++)          // Procura linha com pivot não nulo
    if (ABS(a[k][j]) < TOL)
     imax = k;                       // Troca linha j por linha k
   if (ABS(a[imax][j]) < TOL)
    return 0;                        // não existe solução única para o sistema
  }
  if (imax != j) {                   // Troca linha j por linha imax
   for (k=j; k<n; k++)
    SWAP(a[imax][k], a[j][k]);
   SWAP(b[imax], b[j]);
  }
  for (i=j+1; i<n; i++) {            // Anula elementos abaixo da diagonal
   float aux = a[i][j] / a[j][j];
   for (k=j+1; k<n; k++)             // Transforma os demais elementos da linha
    a[i][k] -= aux * a[j][k];
   b[i] -= aux * b[j];
  }
 }
 if (ABS(a[n-1][n-1]) <= TOL)        // Verifica unicidade da solução
  return 0;                          // Sistema sem solução
 else {
  b[n-1] /= a[n-1][n-1];             // Calcula última coordenada
  for (i=n-2; i>=0; i--) {           // Inicia retro-substituição
   for (j=i+1; j<n; j++)
    b[i] -= a[i][j] * b[j];
   b[i] /= a[i][i];
  }
 }
 return 1;                           // Finaliza a subrotina com solução única
}

int jvFindTransf(float *H, int n0, jvPoint2D *p1, jvPoint2D *p2)
/* Esta rotina forma e resolve um problema da forma
   min || Aq ||
   s.a. ct x = 1
   A solução desejada é a solução do sistema linear
   _	  	_	_	_   _ _     _ _
  | At A | c | | q |   | 0 |
  |_ ct _|_0_| |_w_| = |_1_|
*/
{
 int i, j, k, r;
 int n=n0-1;
 float A[2*MAXPTS][9];
 float M[10][10];
 float X[10];
 float C[12];

 for (i=0; i<2*MAXPTS; i++)
  for (j=0; j<9; j++)
   A[i][j] = 0.0f;
 for (i=0; i<n; i++) {
  A[i+n][3] = A[i][0] = p1[i].x;
  A[i+n][4] = A[i][1] = p1[i].y;
  A[i+n][5] = A[i][2] = 1.0f;
  A[i][6] = -p2[i].x*p1[i].x;
  A[i][7] = -p2[i].x*p1[i].y;
  A[i][8] = -p2[i].x;
  A[i+n][6] = -p2[i].y*p1[i].x;
  A[i+n][7] = -p2[i].y*p1[i].y;
  A[i+n][8] = -p2[i].y;
 }

 for (i=0; i<9; i++) C[i] = 0.0f;
 for (i=0; i<n; i++) {
  C[6] += p1[i].x;
  C[7] += p1[i].y;
 }
 C[6] /= n;
 C[7] /= n;
 C[8] = 1.0f;

 for (i=0; i<10; i++)
  for (j=0; j<10; j++)
   M[i][j] = 0.0f;
 for (i=0; i<9; i++)
  for (j=0; j<9; j++)
   for (k=0; k<2*n; k++)
    M[i][j] += A[k][j]*A[k][i];
 for (i=0; i<9; i++) {
  M[i][9]= C[i];
  M[9][i]= C[i];
 }
 for (i=0; i<9; i++) X[i]= 0.0f;
 X[9] = 1.0f;

 r = Gauss (10, M, X);
 for (i=0; i<9; i++) H[i] = X[i];
 return r;
}

void jvBuildNewData(float minx, float miny, float maxx, float maxy, float *H,
                    int npnx, int npny, jvPoint2D *p1n, jvPoint2D *p2n)
{
 int i, j, k;
 float x, y;
 float dx, dy;
 float zf0;

 dx = (maxx - minx)/(npnx - 1.0f);
 dy = (maxy - miny)/(npny - 1.0f);
 for (i=0, k=0, x=minx; i<npnx; i++, x+=dx)
  for (j=0, y=miny; j<npny; j++, k++, y+=dy) {
   p1n[k].x = x;
   p1n[k].y = y;
   p1n[k].valid = 1;
   zf0 = H[6]*x + H[7]*y + H[8];
   p2n[k].x = (H[0]*x + H[1]*y + H[2])/zf0;
   p2n[k].y = (H[3]*x + H[4]*y + H[5])/zf0;
   p2n[k].valid = 1;
  }
}

void jvClose(void)
{
 if (s_lines_mdl) {
  free(s_lines_mdl); s_lines_mdl = NULL;
 }
 if (s_gaussiana) {
  free(s_gaussiana); s_gaussiana = NULL;
 }

 if (s_pontos && s_pesos) {
  int i;
  for (i=0; i<MAXLINHAS; i++) {
   free(s_pontos[i]);
   free(s_pesos[i]);
  }
  free(s_pontos); s_pontos = NULL;
  free(s_pesos); s_pesos = NULL;
 }

 if (s_ID) {
  free(s_ID); s_ID = NULL;
 }
 if (s_id) {
  free(s_id); s_id = NULL;
 }
}