/*  
    Copyright (C) 2003 Johan Borg

    This file is part of xmerge.

    xmerge is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
                
    xmerge is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
                          
    You should have received a copy of the GNU General Public License
    along with xmerge; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "xmerge.h"
#include <stdio.h>
#include "hqp/Hqp_Conf.h"
#include <math.h>
#include <sys/time.h>

double *x,*p1;

int Goal;	//flags for which parts of the goal-function should be used


//double p2[]= {
//1000,100000,1000,100000,10000,10000,
//10,100,1000,1000,10,100};

double p2[]= {
1000,10000,1000,10000,1000,1000,
100,1000,10000,10000,100,1000};


double p3[]={2,0,0,0,0,0,0,0};

typedef struct p4t0 {
 int si,di;
 double sx,sy,dx,dy;
} p4t0;

typedef struct p4t1 {
 int si,di;
 double sx,sy,dx,dy;
 double I,Q;
} p4t1;


p4t0 *p40;
p4t1 *p41;


con_t *econ;

extern void Hqp_Init(Sqp_Config *conf);

double eps,seps;
#define deps 1e-8

int N,M,M0,M1;

inline double pow2(double x) {
 return x*x;
}

extern double opt_scale;

double obj(double *x) {
 double a,b,c,d,e,g;
 double A,B,C,D,E,G;
 int n;
 double *p,f;
 
 //question is: do we calculate this once and risk loss of accuracy, 2 times, or once and store the results?
 
 A=B=C=D=E=G=0;

  
 for(n=0;n<N;n++) {
  p=x+n*8;
  A+=(p[2]-p[0])/p1[2*n+0];
  B+=(p[0]+p[6]-p[2]-p[4])/p1[2*n+0];
  C+=(p[5]-p[1])/p1[2*n+1];
  D+=(p[1]+p[7]-p[3]-p[5])/p1[2*n+1];
  E+=(p[4]-p[0])/p1[2*n+1];
  G+=(p[3]-p[1])/p1[2*n+0];
 } 
 
 
 A/=N; B/=N; C/=N;
 D/=N; E/=N; G/=N;

 
 a=b=c=d=e=g=0;
 
 for(n=0;n<N;n++) {
  p=x+n*8;
  a+=pow2((p[2]-p[0])/p1[2*n+0]-A);
  b+=pow2((p[0]+p[6]-p[2]-p[4])/p1[2*n+0]-B);
  c+=pow2((p[5]-p[1])/p1[2*n+1]-C);
  d+=pow2((p[1]+p[7]-p[3]-p[5])/p1[2*n+1]-D);
  e+=pow2((p[4]-p[0])/p1[2*n+1]-E);
  g+=pow2((p[3]-p[1])/p1[2*n+0]-G);
 } 

 f=0;
 if (Goal&1)  f=(p2[0]*a+p2[1]*b+p2[2]*c+p2[3]*d+p2[4]*e+p2[5]*g)/N;	
 if (Goal&2)  f+=pow2(A+C-opt_scale)*p2[6];	//scale
 if (Goal&4)  f+=pow2(A-C)*p2[7];		//aspect
 if (Goal&8)  f+=pow2(B)*p2[8];			//x - horizontal stretching 
 if (Goal&16) f+=pow2(D)*p2[9];			//y - vertical stretching 
 if (Goal&32) f+=pow2(E-G)*p2[10];		//rotation
 if (Goal&64) f+=pow2(E+G)*p2[11];		//skewing
 
 return f;
}


//really, not using analytical derivatives is rather silly (and slow, probbably)
void cmap(double *X,double *Y,double x,double y,double *p,int n) {
 double a,b,c,d,e,f,g,h;
 a=(p[2]-p[0])/p1[2*n+0];
 b=(p[0]+p[6]-p[2]-p[4])/p1[2*n+1]/p1[2*n+0];
 c=(p[5]-p[1])/p1[2*n+1];
 d=(p[1]+p[7]-p[3]-p[5])/p1[2*n+1]/p1[2*n+0];
 e=(p[4]-p[0])/p1[2*n+1];
 f=p[0];
 g=(p[3]-p[1])/p1[2*n+0];
 h=p[1];
  
 *Y=y*(c+d*x)+g*x+h;
 *X=x*(a+b*y)+e*y+f;
}



int cnt;
double lfv;

void prog(double *f,double *C,double *B,double * D,double *p,int full){
 int i,j,k1,k2,k;
 double a,b,c,x,y,x1,y1,x2,y2;
 D=0;	//avoids a warning about unused...

 if (full&1) { 
  a=obj(p); 
  for(i=0;i<N*8;i++) {
   b=p[i];
   p[i]+=seps;//*(b+deps);
   c=obj(p);
   p[i]=b;
   c-=a;
   c/=seps;//*(b+deps);
   C[i]=c;
   cnt++;
  } 

  lfv=a;
  *f=a;
 } else {
  *f=lfv=obj(p); 
 }
 
 cnt++;
 if (!(cnt&256)){
  printf("fval: %f  nfeval: %d                 \r",lfv,cnt);
  fflush(stdout);
//  fsync(1);
 } 
 


 if (full&2) {
  k=0;
  for(i=0;i<M0;i++) {
   k1=p40[i].si;
   k2=p40[i].di;
   cmap(&x1,&y1,p40[i].sx,p40[i].sy,p+(k1*8),k1);
   cmap(&x2,&y2,p40[i].dx,p40[i].dy,p+(k2*8),k2);
   B[k  ]=x1-x2;
   B[k+1]=y1-y2;   
   
   for(j=0;j<8;j++) {
    
    a=p[k1*8+j];
    p[k1*8+j]+=seps;//*(a+deps);
   
    cmap(&x,&y,p40[i].sx,p40[i].sy,p+(k1*8),k1);

    p[k1*8+j]=a;
    
    x-=x1;
    y-=y1;

    econ[k  ].der[j]=x/seps;//(a+deps);
    econ[k+1].der[j]=y/seps;//(a+deps);
   } 
   

   for(j=0;j<8;j++) {
    a=p[k2*8+j];
    p[k2*8+j]+=seps;//*(a+deps);

    cmap(&x,&y,p40[i].dx,p40[i].dy,p+(k2*8),k2);

    p[k2*8+j]=a;

    x-=x2;
    y-=y2;
    
    econ[k  ].der[j+8]=-x/seps;//(a+deps);
    econ[k+1].der[j+8]=-y/seps;//(a+deps);
   } 

   k+=2;
  } 
  
  
  for(i=0;i<M1;i++) {
   k1=p41[i].si;
   k2=p41[i].di;
   
   
   cmap(&x1,&y1,p41[i].sx,p41[i].sy,p+(k1*8),k1);
   cmap(&x2,&y2,p41[i].dx,p41[i].dy,p+(k2*8),k2);
   
   
   B[k]=(x2-x1)*p41[i].Q-(y2-y1)*p41[i].I;	//scaling this with respect of the lenght of the vector would be a good idea...
   						//but it would make calculating the derivatives more difficult
   						
   if (k1==k2) {
    x2-=x1;
    y2-=y1;
    for(j=0;j<8;j++) {
     
     a=p[k1*8+j];
     p[k1*8+j]+=seps;//*(a+deps);
    
     cmap(&x,&y,p41[i].sx,p41[i].sy,p+(k1*8),k1);
     x1=-x-x2;
     y1=-y-y2;
     
     cmap(&x,&y,p41[i].dx,p41[i].dy,p+(k1*8),k1);
     x1+=x;
     y1+=y;

 
     p[k1*8+j]=a;
     
     
 
     econ[k].der[j]=(x1*p41[i].Q-y1*p41[i].I)/seps;//(a+deps);
    }
   
   
   
   } else  {
      						
    for(j=0;j<8;j++) {
     
     a=p[k1*8+j];
     p[k1*8+j]+=seps;//*(a+deps);
    
     cmap(&x,&y,p41[i].sx,p41[i].sy,p+(k1*8),k1);
 
     p[k1*8+j]=a;
     
     x-=x1;
     y-=y1;
     
 
     econ[k].der[j]=-(x*p41[i].Q-y*p41[i].I)/seps;//(a+deps);
    } 
   

    for(j=0;j<8;j++) {
     a=p[k2*8+j];
     p[k2*8+j]+=seps;//*(a+deps);
 
     cmap(&x,&y,p41[i].dx,p41[i].dy,p+(k2*8),k2);
 
     p[k2*8+j]=a;
 
     x-=x2;
     y-=y2;
    
     econ[k].der[j+8]=(x*p41[i].Q-y*p41[i].I)/seps;//(a+deps);
    } 
   } 

   k++;
  } 
  
 } else {
  k=0;
  for(i=0;i<M0;i++) {
   k1=p40[i].si;
   k2=p40[i].di;
   cmap(&x1,&y1,p40[i].sx,p40[i].sy,p+(k1*8),k1);
   cmap(&x2,&y2,p40[i].dx,p40[i].dy,p+(k2*8),k2);
   B[k++]=x1-x2;
   B[k++]=y1-y2;   
  }
  for(i=0;i<M1;i++) {
   k1=p41[i].si;
   k2=p41[i].di;
   cmap(&x1,&y1,p41[i].sx,p41[i].sy,p+(k1*8),k1);
   cmap(&x2,&y2,p41[i].dx,p41[i].dy,p+(k2*8),k2);
   B[k++]=(x2-x1)*p41[i].Q-(y2-y1)*p41[i].I;
  }
 }
}


void check(double *x) {
 int n;
 double *p,a,b,c,d,e,g;
 for(n=0;n<N;n++) {
  p=x+n*8;
  a=(p[2]-p[0])/p1[2*n+0];
  b=(p[0]+p[6]-p[2]-p[4])/p1[2*n+1]/p1[2*n+0];
  c=(p[5]-p[1])/p1[2*n+1];
  d=(p[1]+p[7]-p[3]-p[5])/p1[2*n+1]/p1[2*n+0];
  e=(p[4]-p[0])/p1[2*n+1];
  g=(p[3]-p[1])/p1[2*n+0];
  printf("%2.8f %2.8f %2.8f %2.8f %2.8f %2.8f\n",a,b,c,d,e,g);
 } 
}




void update_goal() {	//currently we only disable rotation and skewing, actually
 int a,b=0; 
 double d=0;
 Goal=127;
 for(a=0;a<MAXLINKS;a++) if (rlink[a].stat&2) {
  

  if (rlink[a].type==1) {

   if (b && (d!=rlink[a].angle)) Goal&=~64; else Goal&=~32;
   d=rlink[a].angle; 
   b=1;
  }  
 }
 printf("updating goal function\n");
 if (!(Goal&32)) printf(" removed global rotation from goal function\n");
 if (!(Goal&64)) printf(" removed global skewing from goal function\n");
}






void solve() {
 int a,b,c,d,k1,k2;
 double t,Mrf,Mlf,Mtf,Mbf;
 struct timeval tm;
 struct Sqp_Config conf={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 eps=pow(2,-52);
 seps=pow(eps,0.5)/100;

 update_goal();

 M=0;
 M0=0;
 for(a=0;a<MAXLINKS;a++) if (rlink[a].stat&2) {if (rlink[a].type==1) M++; else { M+=2; M0++; }}

 M1=M-2*M0; 
 
//we restructure the problem somewhat to reduce the overhead when our functions are called by the optimizer 
 
 N=Ot;

 x=malloc(8*N*sizeof(double)*2);
 p1=malloc(2*Ot*sizeof(double)*2);
 if (M0) p40=malloc(M0*sizeof(p4t0)*2); else p40=0;
 if (M1) p41=malloc(M1*sizeof(p4t1)*2); else p41=0;




 econ=conf.econ=malloc(sizeof(con_t)*M*2);
 for(a=0;a<M;a++) econ[a].ind=malloc(sizeof(int)*17*2);
 for(a=0;a<M;a++) econ[a].der=malloc(sizeof(double)*16*2);
 

 
 for(a=0;a<Ot;a++) {
  p1[a*2  ]=simg[a]->sx;
  p1[a*2+1]=simg[a]->sy;
  
  x[a*8+0]=simg[a]->dx1;
  x[a*8+1]=simg[a]->dy1;
  x[a*8+2]=simg[a]->dx2;
  x[a*8+3]=simg[a]->dy2;
  x[a*8+4]=simg[a]->dx3;
  x[a*8+5]=simg[a]->dy3;
  x[a*8+6]=simg[a]->dx4;
  x[a*8+7]=simg[a]->dy4;  
 } 
 

 
 
 
 b=0;
 d=0;
 for(a=0;a<MAXLINKS;a++) if ((rlink[a].stat&2) && (rlink[a].type==0)) {
  for(c=0;c<Ot;c++) if (simg[c]==rlink[a].si) break;
  p40[b].si=c;
  k1=c;
  p40[b].sx=(rlink[a].sx1+rlink[a].sx2)/2.0;
  p40[b].sy=(rlink[a].sy1+rlink[a].sy2)/2.0; 
  for(c=0;c<Ot;c++) if (simg[c]==rlink[a].di) break;
  p40[b].di=c;
  k2=c;
  p40[b].dx=rlink[a].dx+(rlink[a].sx2-rlink[a].sx1)/2.0;
  p40[b].dy=rlink[a].dy+(rlink[a].sy2-rlink[a].sy1)/2.0;
  b++;
   
  for(c=0;c<8;c++) {
   econ[d  ].ind[c]=k1*8+c;
   econ[d+1].ind[c]=k1*8+c;
  } 
   
  for(c=0;c<8;c++) {
   econ[d  ].ind[c+8]=k2*8+c;
   econ[d+1].ind[c+8]=k2*8+c;
  } 
  econ[d++].ind[16]=-1;
  econ[d++].ind[16]=-1;
 }
 
 
 
 b=0;
 for(a=0;a<MAXLINKS;a++) if ((rlink[a].stat&2) && (rlink[a].type==1)) {
  for(c=0;c<Ot;c++) if (simg[c]==rlink[a].si) break;
  p41[b].si=c;
  k1=c;
  p41[b].sx=rlink[a].sx1;
  p41[b].sy=rlink[a].sy1; 
  for(c=0;c<Ot;c++) if (simg[c]==rlink[a].di) break;
  p41[b].di=c;
  k2=c;
  p41[b].dx=rlink[a].dx;
  p41[b].dy=rlink[a].dy;
  p41[b].I=cos(rlink[a].angle);
  p41[b].Q=sin(rlink[a].angle);
  
//  printf("k1: %d k2: %d I: %f Q: %f  %f\n",k1,k2,p41[b].I,p41[b].Q,rlink[a].angle);
  b++;
   
  for(c=0;c<8;c++) econ[d].ind[c  ]=k1*8+c;
  if (k1==k2) econ[d++].ind[8]=-1; else {
   for(c=0;c<8;c++) econ[d].ind[c+8]=k2*8+c;
   econ[d++].ind[16]=-1;  
  } 
 }
 
 printf("d: %d M: %d\n",d,M); 
 
 
 
 conf.prog=&prog;
 conf.x=x;
 conf.n=N*8;
 conf.me=M;
 conf.SqpSolver_max_iters=10000;
 conf.SqpSolver_eps=1e-5;	//going lower may result in Long solution-times, as the derivatives are highly inaccurate at the associated step-lengths
 

 
 cnt=0;

 gettimeofday(&tm,0);
 t=-tm.tv_sec-(float)tm.tv_usec/1000000; 
 Hqp_Init(&conf);
 gettimeofday(&tm,0);
 t+=tm.tv_sec+(float)tm.tv_usec/1000000;
 printf("t: %f\n",t);
 printf("nfeval: %d fval: %f\n",cnt,lfv);
 
 //printf("foo: %f %f %f %f %d\n",x[0],x[1],x[2],x[3],nfev);

 Mbf=Mrf=-10000000;
 Mtf=Mlf= 10000000;
 
 

 for(a=0;a<Ot;a++) {
  simg[a]->dx1=x[a*8+0];
  if (x[a*8+0]>Mrf) Mrf=x[a*8+0]; if (x[a*8+0]<Mlf) Mlf=x[a*8+0];
  simg[a]->dy1=x[a*8+1];
  if (x[a*8+1]>Mbf) Mbf=x[a*8+1]; if (x[a*8+1]<Mtf) Mtf=x[a*8+1];
  simg[a]->dx2=x[a*8+2];
  if (x[a*8+2]>Mrf) Mrf=x[a*8+2]; if (x[a*8+2]<Mlf) Mlf=x[a*8+2];
  simg[a]->dy2=x[a*8+3];
  if (x[a*8+3]>Mbf) Mbf=x[a*8+3]; if (x[a*8+3]<Mtf) Mtf=x[a*8+3];
  simg[a]->dx3=x[a*8+4];
  if (x[a*8+4]>Mrf) Mrf=x[a*8+4]; if (x[a*8+4]<Mlf) Mlf=x[a*8+4];
  simg[a]->dy3=x[a*8+5];
  if (x[a*8+5]>Mbf) Mbf=x[a*8+5]; if (x[a*8+5]<Mtf) Mtf=x[a*8+5];
  simg[a]->dx4=x[a*8+6];
  if (x[a*8+6]>Mrf) Mrf=x[a*8+6]; if (x[a*8+6]<Mlf) Mlf=x[a*8+6];
  simg[a]->dy4=x[a*8+7];  
  if (x[a*8+7]>Mbf) Mbf=x[a*8+7]; if (x[a*8+7]<Mtf) Mtf=x[a*8+7];
 }
 
 
 
  
 
 

 Ml=floor(Mlf);
 Mr=ceil(Mrf); 
 Mt=floor(Mtf); 
 Mb=ceil(Mbf); 
 
 Mr0=Mr;
 Ml0=Ml; 
 Mt0=Mt; 
 Mb0=Mb; 


 free(p40);
 free(p41);
 free(p1);
 free(x); 
 
 for(a=0;a<M;a++) free(econ[a].ind);
 for(a=0;a<M;a++) free(econ[a].der);
 free(econ);
 
// for(a=0;a<N;a++) {
//  for (b=0;b<8;b++) printf("%3.5f ",x[a*8+b]);
//  printf("\n");
// } 
// check(x);
 
}
