#include <stdlib.h>
#include <math.h>
#include <memory.h>
#include "img_dim.h"
#include "edges.h"

#define FATORCOLINEAR 0.9989
#define FATORCOLINEAR2 0.9959

#define ABS(a) (((a)<0)?-(a):(a))
#define MIN(a,b) (((a)<(b))? (a): (b))

#define EPSILON		1.0E-8

int DX, DY;

static GeomPonto2D **pontos = NULL;
static unsigned char **pesos = NULL;
static int *ID = NULL;
static unsigned char *id = NULL;

static size_t s_p2d = sizeof(GeomPonto2D);

static int gaussiana[WIDTHHEIGHT];
static unsigned char cinza[WIDTHHEIGHT];

void ConvolucaoLaplacianadaGaussianaCor(unsigned char *iin, unsigned char *iout, int th)
{
 for (unsigned char *pcinza=cinza, *BGR=iin+1, *BGRMAX=BGR+(3*WIDTHHEIGHT-5); BGR<=BGRMAX; BGR+=3)
  *pcinza++ = *BGR;

 int *p = gaussiana, aux, *pgmax;
 unsigned char *p2 = cinza, *pmax = p2+WIDTH-1,*p2max,
               *p3 = iout, *p3max = &iout[WIDTH];

 for (*p++ = (*(p2+WIDTH) + *(p2+1+WIDTH) + *(p2++) + *(p2+1)) << 2; p2<pmax; p2++)
  *p++ = (*(p2-1+WIDTH) + ((*(p2+WIDTH) + *p2)<<1) + *(p2+1+WIDTH) + *(p2-1) + *(p2+1)) << 1;
 *p++ = (*(p2-1+WIDTH) + *(p2+WIDTH) + *(p2-1) + *p2) << 2;

 for (pmax = p2+(HEIGHT-2)*WIDTH, p2++; p2<pmax; p2++) {
  for (p2max = p2+WIDTH-1, *p++ = (*(p2+WIDTH) + *(p2+1+WIDTH) + ((*(p2++) + *(p2+1))<<1) + *(p2-WIDTH) + *(p2+1-WIDTH)) << 1; p2<p2max; p2++)
   *p++ = *(p2-1+WIDTH) + ((*(p2+WIDTH) + *(p2-1) + *(p2+1) + *(p2-WIDTH))<<1) + *(p2+1+WIDTH) + ((*p2)<<2) + *(p2-1-WIDTH) + *(p2+1-WIDTH);
  *p++ = (*(p2-1+WIDTH) + *(p2+WIDTH) + ((*(p2-1) + *p2)<<1) + *(p2-1-WIDTH) + *(p2-WIDTH)) << 1;
 }

 for (pmax = p2+WIDTH-1, *p++ = (*p2 + *(p2+1) + *((p2++)-WIDTH) + *(p2+1-WIDTH)) << 2; p2<pmax; p2++)
  *p++ = (*(p2-1) + *(p2+1) + ((*(p2-WIDTH) + *p2)<<1) + *(p2-1-WIDTH) + *(p2+1-WIDTH)) << 1;
 *p = (*(p2-1) + *p2 + *(p2-1-WIDTH) + *(p2-WIDTH)) << 2;

 for (; p3<p3max; *p3++ = 0);
 for (p = &gaussiana[WIDTH], p3max = p3+(HEIGHT-2)*WIDTH; p3<p3max; p++) {
  for (*p3++ = 0, pgmax = p+WIDTH-1, p++; p<pgmax; p++) 
   *p3++ = (unsigned char)(((aux = ((*p)<<2) - (*(p+WIDTH) + *(p-1) + *(p+1) + *(p-WIDTH))) <= th)? 0 : 255);
  *p3++ = 0;
 }
 memset(p3, 0, WIDTH);
}

static int direcaoimagem(unsigned char *iin, int x1, int y1, float fator, float &vx, float &vy)
{
 int x2 = x1+DX, y2 = y1+DY,
     xmaux = 0, ymaux = 0, cont=0, i;
 unsigned char *piin, *piin0 = iin+y1*WIDTH;
 for (int j=y1; j<y2; j++, piin0 += WIDTH)
  for (i=x1, piin = piin0+x1; i<x2; i++)
   if (*piin++) {
    xmaux += i,
    ymaux += j,
    cont++;
   }
 if (cont<5) return 0;

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

 int cont2=0;
 float a = 0.0, b = 0.0, c = 0.0,
       i2, i2max, j2 = y1-ym;
 for (piin0 = iin+y1*WIDTH; cont2<cont; j2++, piin0 += WIDTH) {
  float b1 = 0.0;
  int c1 = 0;
  for (i2=x1-xm, i2max=x2-xm, piin=piin0+x1; i2<i2max; i2++)
   if (*piin++) {
    a += i2*i2,
    b1 += i2,
    c1++,
    cont2++;
    if (cont2 == cont) break;
   }
  b += b1*j2, c += c1*j2*j2;
 }

 float 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.0, vy = 1.0;
  return 1;
 } else if (c < EPSILON) {
  vx = 1.0, vy = 0.0;
  return 1;
 } else if (ABS(b) < EPSILON) {
  if (amc > 0) {
   vx = 1.0, vy = 0.0;
   return 1;
  } else {
   vx = 0.0, vy = 1.0;
   return 1;
  }
 }
 vx = (float)((amc+delta)/(2.0*b)),
 vy = (float)(1.0/sqrt(vx*vx + 1.0)),
 vx *= vy;
 return 1;
}

static int direcaoimagem(unsigned char *iin, int x1, int y1, int x3, int y3, float fator, float &vx, float &vy)
{
 int i, j, x2 = x1+DX, y2 = y1+DY, x4 = x3+DX, y4 = y3+DY,
     xmaux = 0, ymaux = 0, cont=0;
 unsigned char *piin, *piin0 = iin+y1*WIDTH;
 for (j=y1; j<y2; j++, piin0 += WIDTH)
  for (i=x1, piin = piin0+x1; i<x2; i++)
   if (*piin++) {
    xmaux += i,
    ymaux += j,
    cont++;
   }
 for (piin0 = iin+y3*WIDTH, j=y3; j<y4; j++, piin0 += WIDTH)
  for (i=x3, piin = piin0+x3; i<x4; i++)
   if (*piin++) {
    xmaux += i,
    ymaux += j,
    cont++;
   }
 float xm = xmaux/((float)cont), ym = ymaux/((float)cont);

 int cont2=0;
 float a = 0.0, b = 0.0, c = 0.0,
       i2, i2max, j2 = y1-ym, j2max = y2-ym;
 for (piin0 = iin+y1*WIDTH; j2<j2max; j2++, piin0 += WIDTH) {
  float b1 = 0.0;
  int c1 = 0;
  for (i2=x1-xm, i2max=x2-xm, piin=piin0+x1; i2<i2max; i2++)
   if (*piin++) {
    a += i2*i2,
    b1 += i2,
    c1++,
    cont2++;
   }
  b += b1*j2, c += c1*j2*j2;
 }
 j2 = y3-ym;
 for (piin0 = iin+y3*WIDTH; cont2<cont; j2++, piin0 += WIDTH) {
  float b1 = 0.0;
  int c1 = 0;
  for (i2=x3-xm, i2max=x4-xm, piin=piin0+x3; i2<i2max; i2++)
   if (*piin++) {
    a += i2*i2,
    b1 += i2,
    c1++,
    cont2++;
    if (cont2 == cont) break;
   }
  b += b1*j2, c += c1*j2*j2;
 }

 float 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.0, vy = 1.0;
  return 1;
 } else if (c < EPSILON) {
  vx = 1.0, vy = 0.0;
  return 1;
 } else if (ABS(b) < EPSILON) {
  if (amc > 0) {
   vx = 1.0, vy = 0.0;
   return 1;
  } else {
   vx = 0.0, vy = 1.0;
   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(GeomPonto2D *p1, GeomPonto2D *pm, TreeInterpLinha *linha)
{
 float x2 = linha->v2x - linha->v1x,
       y2 = linha->v2y - linha->v1y,
       pi = p1->x*x2+p1->y*y2;
 if (pi*pi < FATORCOLINEAR2*(x2*x2 + y2*y2)) return 0;

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

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

static void ExtraiPontos(unsigned char *imagem, int x1, int y1, GeomPonto2D *pontos, int &npontos, unsigned char *pesos)
{
 unsigned char *p;
 int i, j;
 GeomPonto2D *pp=&pontos[npontos];
 unsigned char *ppesos=&pesos[npontos],
               *p0 = imagem+y1*WIDTH;
 for (j=y1; j<y1+DY; j++, p0 += WIDTH)
  for (i=x1, p=p0+x1; i<x1+DX; i++, p++)
   if (*p) {
    *ppesos++ = *p,
    pp->x = (float)i, pp->y = (float)j,
    pp++,
    npontos++;
   }
}

void ExtraiLinha(GeomPonto2D *pontos, int npontos, TreeInterpLinha &l)
{
 GeomPonto2D *pt = pontos;
 float mx=pt[0].x, my=pt[0].y, Mx=pt[0].x, My=pt[0].y;

 GeomPonto2D *paux=&pontos[npontos-1];
 for (pt++; pt<=paux; pt++) {
  if (pt->x < mx) mx = pt->x;
  else if (pt->x > Mx) Mx = pt->x;
  if (pt->y < my) my = pt->y;
  else if (pt->y > My) My = pt->y;
 }

 if (Mx-mx < 2) {
  l.v1x = 0.5f*(Mx+mx), l.v1y = my,
  l.v2x = l.v1x,        l.v2y = My;
  return;
 }

 float sxy=0, sxx=0, sy=0, sx=0;
 for (pt = pontos; pt<=paux; pt++) {
  sx += pt->x,
  sy += pt->y,
  sxy += pt->x*pt->y,
  sxx += pt->x*pt->x;
 }

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

 GeomPonto2D p[4];
 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 (int i=0; i<3; i++)
  for (int j=i+1; j<4; j++)
   if (p[j].x < p[i].x) {
    GeomPonto2D 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;
}

void ExtraiLinha(GeomPonto2D *pontos, unsigned char *pesos, int npontos, TreeInterpLinha &l)
{
 GeomPonto2D *pt = pontos;
 float mx=pt[0].x, my=pt[0].y, Mx=pt[0].x, My=pt[0].y;

 GeomPonto2D *paux=&pontos[npontos-1];
 for (pt++; pt<=paux; pt++) {
  if (pt->x < mx) mx = pt->x;
  else if (pt->x > Mx) Mx = pt->x;
  if (pt->y < my) my = pt->y;
  else if (pt->y > My) My = pt->y;
 }

 if (Mx-mx < 2) {
  l.v1x = 0.5f*(Mx+mx), l.v1y = my,
  l.v2x = l.v1x,        l.v2y = My;
  return;
 }

 int npaux = 0;
 unsigned char *ps = pesos;
 float sxy=0, sxx=0, sy=0, sx=0, ptxps;
 for (pt = pontos; pt<=paux; pt++, ps++) {
  ptxps = pt->x * *ps,
  sx += ptxps,
  sy += pt->y * *ps,
  sxy += pt->y * ptxps,
  sxx += pt->x * ptxps,
  npaux += *ps;
 }

 float a, aux;
 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;
 float b = (sy - sx*a)/npaux;

 GeomPonto2D p[4];
 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 (int i=0; i<3; i++)
  for (int j=i+1; j<4; j++)
   if (p[j].x < p[i].x) {
    GeomPonto2D 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;
}

#define MAXLINHAS 40

static int UniaoLinhas (unsigned char *iin, int *ID, unsigned char *Id, int idmax, TreeInterpLinha *linha)
{
 int i, j;

 if (pontos == NULL) {
  pontos = new GeomPonto2D* [MAXLINHAS];
  pesos = new unsigned char* [MAXLINHAS];
  for (i=0; i<MAXLINHAS; i++) {
   pontos[i] = new GeomPonto2D [WIDTHHEIGHT];
   pesos[i] = new unsigned char [WIDTHHEIGHT];
  }
 }

 int npontos[MAXLINHAS];
 unsigned char *pid = &Id[1];
 GeomPonto2D **pp = pontos;
 unsigned char **ppesos = pesos;
 int *pnp = npontos;
 TreeInterpLinha *l1 = linha;
 int nlinhas = 0;
 int *IDINI = &ID[WIDTH/DX];
 for (int id=1; id<idmax; id++, pid++) 
  if (*pid > 1) {
   int cont=0;
   *pnp = 0;
   int *pID = IDINI;
   for (j=DY; cont<*pid; j+=DY, pID++)
    for (i=DX, pID++; i<WIDTH-DX; i+=DX)
     if (*pID++ == id) {
      ExtraiPontos(iin, i, j, *pp, *pnp, *ppesos);
      cont++;
      if (cont == *pid) break;
     }
   ExtraiLinha(*pp++, *ppesos++, *pnp++, *l1++);
   nlinhas++;
  }

 if (nlinhas == 0) return 0;

 TreeInterpLinha linha2[MAXLINHAS];
 l1 = linha;
 TreeInterpLinha *la2 = linha2;
 int nlinhas2 = 0;
 TreeInterpLinha *l2;
 GeomPonto2D **pp2;
 unsigned char **ppesos2;
 int *np2;
 GeomPonto2D d, p;
 for (i=0, pp=pontos, ppesos=pesos, pnp=npontos; i<nlinhas-1; i++, l1++, pp++, pnp++) 
  if (*pnp) {
   int flag = 0;
   d.x = l1->v2x - l1->v1x;
   d.y = l1->v2y - l1->v1y;
   float normal1 = (float)sqrt(d.x*d.x + d.y*d.y);
   d.x /= normal1; d.y /= normal1;
   p.x = (l1->v1x + l1->v2x)*0.5f;
   p.y = (l1->v1y + l1->v2y)*0.5f;
   for (j=i+1, l2=l1+1, pp2=pp+1, ppesos2=ppesos+1, np2=pnp+1; j<nlinhas; j++, l2++, pp2++, ppesos2++, np2++)
    if (*np2!=0 && colinear(&d, &p, l2)) {
     flag = 1;
     memcpy(&(*pp)[*pnp], *pp2, *np2*s_p2d);
     memcpy(&(*ppesos)[*pnp], *ppesos2, *np2);
     *pnp += *np2;
     *np2 = 0;
    }
   if (flag) ExtraiLinha(*pp, *pnp, *la2++);
   else *la2++ = *l1;
   nlinhas2++;
  }
 if (npontos[nlinhas-1] != 0) {
  *la2++ = *l1;
  nlinhas2++;
 }
 nlinhas = nlinhas2;
 memcpy(linha, linha2, nlinhas*sizeof(TreeInterpLinha));

 return nlinhas;
}

int edgedetect(unsigned char *iin, int dx, int dy, float fator, TreeInterpLinha *linha)
{
 DX = dx; DY = dy;
 int lcel = WIDTH/dx, acel = HEIGHT/dy;
 float vx, vy;
 if (ID != NULL) {
  delete [] ID; ID = NULL;
 }
 if (id != NULL) {
  delete [] id; id = NULL;
 }
 ID = new int [lcel*acel];
 id = new unsigned char [lcel*acel];
 memset(id, 0, lcel*acel);
 int cont = 1;

 int pos = 0;
 for (; pos<lcel; ID[pos++] = 0);
 for (int j=dy; j<HEIGHT-DY; j+=dy) {
  ID[pos++] = 0;
  for (int i=dx; i<WIDTH-DX; i+=dx, pos++)
   if (direcaoimagem(iin, i, j, fator, vx, vy)) {
    float vx2, vy2;
    if      (vx >= 0 && 
             ID[pos - 1 - lcel] != 0 &&
             direcaoimagem(iin, i, j, i-dx, j-dy, fator, vx2, vy2) && 
             (vx*vx2 + vy*vy2) > FATORCOLINEAR)
     ID[pos] = ID[pos - 1 - lcel];
    else if (ID[pos - lcel] != 0 &&
             direcaoimagem(iin, i, j, i, j-dy, fator, vx2, vy2) && 
             (vx*vx2 + vy*vy2) > FATORCOLINEAR)
     ID[pos] = ID[pos - lcel];
    else if (vx <= 0 && 
             ID[pos + 1 - lcel] != 0 &&
             direcaoimagem(iin, i, j, i+dx, j-dy, fator, vx2, vy2) && 
             (vx*vx2 + vy*vy2) > FATORCOLINEAR)
     ID[pos] = ID[pos + 1 - lcel];
    else if (ID[pos - 1] != 0 &&
             direcaoimagem(iin, i, j, i-dx, j, fator, vx2, vy2) && 
             (vx*vx2 + vy*vy2) > FATORCOLINEAR)
     ID[pos] = ID[pos - 1];
    else ID[pos] = cont++;
    id[ID[pos]]++;
   } else ID[pos] = 0;
   ID[pos++] = 0;
 }

 if (cont < 5) return 0;
 return UniaoLinhas (iin, ID, id, cont, linha);
}

void DesalocaMemoria()
{
 if (pontos != NULL) {
  for (int i=0; i<MAXLINHAS; i++) {
   delete [] pontos[i];
   delete [] pesos[i];
  }
  delete [] pontos;
  delete [] pesos;
 }

 if (ID != NULL) delete [] ID;
 if (id != NULL) delete [] id;
}