#include <math.h>
#include "verification.h"
#include "gauss.h"

#define LARGE_TOL 2

static Boolean calc_pre_verif_coef(double *x1,double *y1,double *x2,double *y2,double *coef);
static Boolean revise_coef(int nref, Leaf *ref, double *coef);
static Boolean least_square_tran(int num, double *x1, double *y1, double *x2, double *y2, double *coef);
static void get_xy_sum1(int num, double *x, double *y, double *Sx, double *Sy, double *Sxx, double *Sxy, double *Syy);
static void get_xy_sum2(int num, double *x, double *y, double *z, double *Sz, double *Szx, double *Szy);
static void leaf_tran(int ndata, Leaf *data, double *coef);
static void combine_tran(double *coef, double *coef2);

Boolean pre_verif(triangle p, triangle q, double *coef, double limit){
   double x1[3], y1[3], x2[3], y2[3], size; //, rotate, delta;
   x1[0]=get_xym_x(get_triangle_a(p)); y1[0]=get_xym_y(get_triangle_a(p));
   x1[1]=get_xym_x(get_triangle_b(p)); y1[1]=get_xym_y(get_triangle_b(p));
   x1[2]=get_xym_x(get_triangle_c(p)); y1[2]=get_xym_y(get_triangle_c(p));
   x2[0]=get_xym_x(get_triangle_a(q)); y2[0]=get_xym_y(get_triangle_a(q));
   x2[1]=get_xym_x(get_triangle_b(q)); y2[1]=get_xym_y(get_triangle_b(q));
   x2[2]=get_xym_x(get_triangle_c(q)); y2[2]=get_xym_y(get_triangle_c(q));
   if(calc_pre_verif_coef(x1,y1,x2,y2,coef)!=TRUE) return FALSE;
   size=sqrt(pow(coef[0],2)+pow(coef[1],2));
   if(fabs(coef[0]-coef[4])>limit*size) return FALSE;
   if(fabs(coef[1]+coef[3])>limit*size) return FALSE;
   if(fabs(size-1)>3*limit) return FALSE;
   return TRUE;
}

static Boolean calc_pre_verif_coef(double *x1,double *y1,double *x2,double *y2,double *coef){
   int i;
   double a[36], b[6];
   a[0]=x1[0];  a[1]=y1[0];  a[2]=1;   a[3]=0;      a[4]=0;      a[5]=0;
   a[6]=x1[1];  a[7]=y1[1];  a[8]=1;   a[9]=0;      a[10]=0;     a[11]=0;
   a[12]=x1[2]; a[13]=y1[2]; a[14]=1;  a[15]=0;     a[16]=0;     a[17]=0;
   a[18]=0;     a[19]=0;     a[20]=0;  a[21]=x1[0]; a[22]=y1[0]; a[23]=1;
   a[24]=0;     a[25]=0;     a[26]=0;  a[27]=x1[1]; a[28]=y1[1]; a[29]=1;
   a[30]=0;     a[31]=0;     a[32]=0;  a[33]=x1[2]; a[34]=y1[2]; a[35]=1;
   b[0]=x2[0]; b[1]=x2[1]; b[2]=x2[2];
   b[3]=y2[0]; b[4]=y2[1]; b[5]=y2[2];
   if(gauss(6,a,b)!=TRUE){
      fprintf(stderr,"# gauss solution failed!\n"); return FALSE;
   }
   for(i=0;i<6;i++) coef[i]=b[i];
   return TRUE;
}

Boolean verification(FILE *flog, int nobj, xym *data_obj, int nref, xym *data_ref, double *coef, opm_param *setting){
   int nmatch, ntotal, i, niter;
   double ratio, coef2[6], xcen, ycen, size, radius, matching;
   Leaf *ref, *obj;
   Boolean matched;
   niter=get_opm_niter(setting);
   if(niter<0){
      fprintf(stderr,"!!! verification : niter<0 !!!\n"); exit(1);
   }
   obj=make_leaf(nobj,data_obj);
   leaf_tran(nobj,obj,coef);
   ref=make_leaf(nref,data_ref);
   xcen=get_opm_xcen(setting);
   ycen=get_opm_ycen(setting);
   size=get_opm_size(setting);
   radius=get_opm_radius(setting);
   matching=get_opm_matching(setting);
   make_treematch_pre(nobj,obj,nref,ref,xcen,ycen,size,LARGE_TOL*radius);
   nmatch=count_adopted(nref,ref);
   if(nmatch<5) goto finish;
   if(nobj>nref) ntotal=nref;
   else ntotal=nobj;
   for(i=0;i<niter;i++){
      if(revise_coef(nref,ref,coef2)!=TRUE) goto finish;
      leaf_tran(nobj,obj,coef2);
      combine_tran(coef,coef2);
      //make_treematch_pre(nobj,obj,nref,ref,xcen,ycen,size,radius);
      make_treematch(nobj,obj,nref,ref,xcen,ycen,size,radius);
      nmatch=count_adopted(nref,ref);
      ratio=(double)nmatch / (double)ntotal;
      if(nmatch<5) goto finish;
   }
   if(niter<=0) make_treematch(nobj,obj,nref,ref,xcen,ycen,size,radius);
finish:
   for(i=0;i<nobj;i++) free(obj[i]);
   for(i=0;i<nref;i++) free(ref[i]);
   free(obj); free(ref);
   if(nmatch>5&&ratio>=matching){
      matched=TRUE;
      xym_tran(nobj,data_obj,coef);
   } else matched=FALSE;
   if(flog!=NULL){
      if(matched==TRUE) fprintf(flog,"# verification success (radius=%.2f matching=%.2f ratio=%.2f)\n",radius,matching,ratio);
      else fprintf(flog,"# verification failure (radius=%.2f matching=%.2f ratio=%.2f)\n",radius,matching,ratio);
   }
   return matched;
}

Leaf *make_leaf(int ndata, xym *data){
   int i;
   Leaf *tmp;
   xym p;
   double x, y, m;
   if((tmp=malloc(ndata*sizeof(Leaf)))==NULL){
      fprintf(stderr,"malloc failed in make_Leaf !\n"); exit(1);
   }
   for(i=0;i<ndata;i++){
      p=data[i];
      x=get_xym_x(p); y=get_xym_y(p); m=get_xym_m(p);
      tmp[i]=create_Leaf(x,y,m,p);
   }
   return tmp;
}

/*
Leaf *make_leaf_tran(int ndata, xym *data, double *coef){
   int i;
   Leaf *tmp;
   xym p;
   double x, y, m;
   if((tmp=malloc(ndata*sizeof(Leaf)))==NULL){
      fprintf(stderr,"malloc failed in make_Leaf !\n"); exit(1);
   }
   for(i=0;i<ndata;i++){
      p=data[i];
      x=coef[0]*get_xym_x(p)+coef[1]*get_xym_y(p)+coef[2];
      y=coef[3]*get_xym_x(p)+coef[4]*get_xym_y(p)+coef[5];
      m=get_xym_m(p);
      tmp[i]=create_Leaf(x,y,m,p);
   }
   return tmp;
}
*/

static Boolean revise_coef(int nref, Leaf *ref, double *coef){
   int i, num;
   Leaf p, q;
   double *x1, *y1, *x2, *y2;
   Boolean tmp;
   x1=malloc(nref*sizeof(double));
   y1=malloc(nref*sizeof(double));
   x2=malloc(nref*sizeof(double));
   y2=malloc(nref*sizeof(double));
   for(i=0,num=0;i<nref;i++){
      p=ref[i];
      if(p->nmatch<=0) continue;
      q=p->match[0];
      x1[num]=q->x; y1[num]=q->y; x2[num]=p->x; y2[num]=p->y;
      ++num;
   }
   tmp=least_square_tran(num,x1,y1,x2,y2,coef);
   free(x1); free(y1); free(x2); free(y2);
   return tmp;
}

static Boolean least_square_tran(int num, double *x1, double *y1, double *x2, double *y2, double *coef){
   int i;
   double a[9], b[3], tmp_coef[6];
   double Sx, Sy, Sxx, Sxy, Syy, Sx2, Sx2x, Sx2y, Sy2x, Sy2, Sy2y;
   get_xy_sum1(num,x1,y1,&Sx,&Sy,&Sxx,&Sxy,&Syy);
   a[0]=Sxx; a[1]=Sxy; a[2]=Sx;
   a[3]=Sxy; a[4]=Syy; a[5]=Sy;
   a[6]=Sx;  a[7]=Sy;  a[8]=num;
   get_xy_sum2(num,x1,y1,x2,&Sx2,&Sx2x,&Sx2y);
   b[0]=Sx2x; b[1]=Sx2y; b[2]=Sx2;
   if(gauss(3,a,b)!=TRUE) return FALSE;
   for(i=0;i<3;i++) tmp_coef[i]=b[i];
   a[0]=Sxx; a[1]=Sxy; a[2]=Sx;
   a[3]=Sxy; a[4]=Syy; a[5]=Sy;
   a[6]=Sx;  a[7]=Sy;  a[8]=num;
   get_xy_sum2(num,x1,y1,y2,&Sy2,&Sy2x,&Sy2y);
   b[0]=Sy2x; b[1]=Sy2y; b[2]=Sy2;
   if(gauss(3,a,b)!=TRUE) return FALSE;
   for(i=0;i<3;i++) tmp_coef[i+3]=b[i];
   for(i=0;i<6;i++) coef[i]=tmp_coef[i];
   return TRUE;
}

static void get_xy_sum1(int num, double *x, double *y, double *Sx, double *Sy, double *Sxx, double *Sxy, double *Syy){
   int i;
   double tmpSx, tmpSy, tmpSxx, tmpSxy, tmpSyy;
   tmpSx=0; tmpSy=0; tmpSxx=0; tmpSxy=0; tmpSyy=0;
   for(i=0;i<num;i++){
      tmpSx+=x[i];
      tmpSy+=y[i];
      tmpSxx+=pow(x[i],2);
      tmpSxy+=(x[i]*y[i]);
      tmpSyy+=pow(y[i],2);
   }
   *Sx=tmpSx; *Sy=tmpSy; *Sxx=tmpSxx; *Sxy=tmpSxy; *Syy=tmpSyy;
}

static void get_xy_sum2(int num, double *x, double *y, double *z, double *Sz, double *Szx, double *Szy){
   int i;
   double tmpSz, tmpSzx, tmpSzy;
   tmpSz=0; tmpSzx=0; tmpSzy=0;
   for(i=0;i<num;i++){
      tmpSz+=z[i];
      tmpSzx+=(z[i]*x[i]);
      tmpSzy+=(z[i]*y[i]);
   }
   *Sz=tmpSz; *Szx=tmpSzx; *Szy=tmpSzy;
}

static void leaf_tran(int ndata, Leaf *data, double *coef){
   int i;
   Leaf p;
   double xold, yold;
   for(i=0;i<ndata;i++){
      p=data[i];
      xold=p->x; yold=p->y;
      p->x=coef[0]*xold+coef[1]*yold+coef[2];
      p->y=coef[3]*xold+coef[4]*yold+coef[5];
   }
}

static void combine_tran(double *coef, double *coef2){
   int i;
   double tmp[6];
   for(i=0;i<6;i++) tmp[i]=coef[i];
   coef[0]=tmp[0]*coef2[0]+tmp[3]*coef2[1];
   coef[1]=tmp[1]*coef2[0]+tmp[4]*coef2[1];
   coef[2]=tmp[2]*coef2[0]+tmp[5]*coef2[1]+coef2[2];
   coef[3]=tmp[0]*coef2[3]+tmp[3]*coef2[4];
   coef[4]=tmp[1]*coef2[3]+tmp[4]*coef2[4];
   coef[5]=tmp[2]*coef2[3]+tmp[5]*coef2[4]+coef2[5];
}

void calc_coef_param(Boolean mirror, double *coef, double *magnify, double *angle){
   int i;
   double tmp[6], mag1, mag2, angle1, angle2;
   for(i=0;i<6;i++) tmp[i]=coef[i];
   if(mirror==TRUE){ tmp[0]*=(-1); tmp[3]*=(-1); }
   mag1=sqrt(pow(tmp[0],2)+pow(tmp[1],2));
   mag2=sqrt(pow(tmp[3],2)+pow(tmp[4],2));
   angle1=180*atan2(-tmp[1],tmp[0])/M_PI;
   angle2=180*atan2(tmp[3],tmp[4])/M_PI;
   if(fabs(angle1-angle2)>2*ANGLE_LIMIT){
      if(angle1<0) angle1+=360;
      else if(angle2<0) angle2+=360;
   }
   *magnify=(mag1+mag2)/2.0;
   *angle=(angle1+angle2)/2.0;
}
