#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include "treeinterp.h"

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

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

static float *compr = NULL;
static float cmax = 0.0;
static unsigned char *R = NULL;
static int nlinhasi;
static TreeInterpLinha *linhai;

static int NLINHAS;
static TreeInterpLinha *LINHA;

static PFNRESTRICAOPROC restricao;

static int sinall(TreeInterpLinha l1, TreeInterpLinha l2)
{
 float S1, S2, V1, aux;
 V1 = 5*l2.compr2;
 aux = l2.v1x*l2.v2y - l2.v2x*l2.v1y;
 S1 = aux + l1.v1y*l2.dx - l1.v1x*l2.dy;
 if (SQR(S1) < V1) S1 = 0.0;
 S2 = aux + l1.v2y*l2.dx - l1.v2x*l2.dy;
 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(TreeInterpLinha l1, TreeInterpLinha l2)
{
 float S1, S2, aux;
 aux = l2.v1x*l2.v2y - l2.v2x*l2.v1y;
 S1 = aux + l1.v1y*l2.dx - l1.v1x*l2.dy;
 if (SQR(S1) < 1e-7) S1 = 0.0;
 S2 = aux + l1.v2y*l2.dx - l1.v2x*l2.dy;
 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;
}

#define FATORPERSPECTIVA_Y 2.0f
static int dirl(TreeInterpLinha l1, TreeInterpLinha l2)
{
 float b = FATORPERSPECTIVA_Y*l1.dy,
       d = FATORPERSPECTIVA_Y*l2.dy,
       e = l1.dx*l2.dx + b*d;
 return e*e > 0.65f*(l1.dx*l1.dx+b*b)*(l2.dx*l2.dx+d*d);
// return e*e > 0.95f*(l1.dx*l1.dx+b*b)*(l2.dx*l2.dx+d*d);
}

#define dirL(l1, l2) ((ABS(l1.dx)<1e-5 && ABS(l2.dx)<1e-5) || (ABS(l1.dy)<1e-5 && ABS(l2.dy)<1e-5))

static int Restricoes(int f, unsigned char *F)
{
 int i, j, k;
 TreeInterpLinha lf, LF, *l;
 unsigned char *Ff = &F[f];

 int cont=0;
 unsigned char *Fi = F;

 /* se a interpretacao da linha corrente for nula, nao tem o que testar */
 if (*Ff == 255) return 1;

 /* impede que duas linhas possam ser interpretadas como a mesma linha do modelo */
 for (Fi=F; Fi<Ff; )
  if (*Fi++ == *Ff)
   return 0; // Impede a repeticao

 /* 
    Teste de paralelismo: 
     Dois segmentos paralelos tem que ter seus correspondentes paralelos
     Dois segmentos nao paralelos tem que ter seus correspondentes nao paralelos
  */
 lf=linhai[f];
 LF=LINHA[*Ff];
 l=linhai;
 for (Fi=F; Fi<=Ff; Fi++, l++)
  if ((*Fi != 255) && (dirl(*l, lf) != dirL(LINHA[*Fi], LF)))
   return 0; 

 /* Teste de ordem relativa dos segmentos */
 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(LINHA[F[j]], LINHA[F[i]]);
     if (a1 != 0) {
      int a3 = sinall(linhai[j], linhai[i]);
      if (a3==0) return 0;
      for (k=0; k<=f; k++) 
       if (k!=i && k!=j && F[k]!=255) {
        int a2 = sinalL(LINHA[F[k]], LINHA[F[i]]);
        int a4 = sinall(linhai[k], linhai[i]);
        if ((a4==0 && a2!=0) || (a2!=0 && (a1*a2 != a3*a4))) return 0;
       }
     }
    }
   }
 return 1;
}

static float Comprimento(unsigned char *F)
{
 unsigned char i, *pF;
 for (i=0, pF=F; i<nlinhasi; i++, pF++)
  if (*pF == 0) return compr[i];
 return 0;
}

static void BuscaEmArvore(int f, unsigned char *F, float c)
{
 unsigned char NF;
 if (f == nlinhasi) {
  if ( (restricao==NULL) || restricao(nlinhasi, linhai, F))
   if (c>cmax || (c==cmax && Comprimento(R)<Comprimento(F))) {
    memcpy(R, F, f);
    cmax = c;
   }
 } else {
  F[f] = 255;
  if (Restricoes(f, F)) 
   BuscaEmArvore(f+1, F, c);
  for (NF=0; NF<NLINHAS; NF++) {
   F[f] = NF;
   if (Restricoes(f, F)) 
    BuscaEmArvore(f+1, F, c + compr[f]);
  }
 }
}

void TreeInterpretation(int nlinhas_mdl, TreeInterpLinha *linha_mdl,
                        int nlinhas, TreeInterpLinha *linha, 
                        PFNRESTRICAOPROC restricao_function, unsigned char *res)
{
 float *ci;
 TreeInterpLinha *li, *limax;
 unsigned char *F, NF;
 NLINHAS = nlinhas_mdl; LINHA = linha_mdl;
 nlinhasi = nlinhas; linhai = linha;
 restricao = restricao_function;
 for (li=LINHA, limax=&LINHA[NLINHAS-1]; li<=limax; li++) {
  li->dx=li->v2x - li->v1x;
  li->dy=li->v2y - li->v1y;
 }
 compr = (float *)malloc(nlinhasi*sizeof(float));
 ci = compr;
 for (li=linhai, limax=&linhai[nlinhasi-1]; li<=limax; li++) {
  li->dx=li->v2x - li->v1x;
  li->dy=li->v2y - li->v1y;
  li->compr2 = li->dx*li->dx + li->dy*li->dy;
  *ci++ = (float)sqrt(li->compr2);
 }
 cmax = 0.0;
 F = (unsigned char *)malloc((nlinhasi+1)*sizeof(unsigned char));
 R = res;
 F[0] = 255;
 BuscaEmArvore(1, F, 0);
 for (NF=0; NF<NLINHAS; NF++) {
  F[0] = NF;
  BuscaEmArvore(1, F, compr[0]);
 }
 free(F);
 free(compr);
}