/*->c.simplex */


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


#include "h.os"



#include "h.simplex"


#define M      4
#define N      M+1
#define ALPHA  1.0
#define BETA   0.5
#define GAMMA  2.0
#define LW     5
#define ROOT2  1.414214

typedef int vector[N];


#define until(x) while (!(x))

int       h[N], l[N];
int       np, maxiter, niter;
vector    next, mean, error, maxerr, step, simp[N];


/*****************************************************************************/



void order(void)
{
 int i;
 int j;

 for(j=0;j<N;j++)
  for(i=0;i<N;i++)
  {
   if(simp[i][j]<simp[l[j]][j]) l[j] = i;
   if(simp[i][j]>simp[h[j]][j]) h[j] = i;
  }
}




#define sqr(x) ((x) * (x))


os_error * sum_residual(vector x)
{
 os_error * err;

 err=sresiduals(x[0],x[1],x[2],x[3],&x[N-1]);

 return(err);
}




void new_vertex(void)
{
 int   i;

 for(i=0;i<N;i++)
 {
  simp[h[N-1]][i]=next[i];
 }
}




/* we pass our guesses for the parameters */

os_error * simplex(int param1,int param2,int param3,int param4,int * done)
{
 int    i;
 int    j;
 vector center,p,q;
 os_error * err;

 simp[0][0]=param1;
 simp[0][1]=param2;
 simp[0][2]=param3;
 simp[0][3]=param4;

 maxiter=50;
 step[0]=param1/8;
 step[1]=param2/8;
 step[2]=param3/8;
 step[3]=param4/8;

 if(step[0]<spddx) step[0]=spddx;
 if(step[1]<spddy) step[1]=spddy;
 if(step[2]<spddx) step[2]=spddx;
 if(step[3]<spddy) step[3]=spddy;


 /* First Vertex */
 err=sum_residual(simp[0]);
 if(err) return(err);

 /* Compute offset of Vertices */
 for(i=0;i<M;i++)
 {
  p[i]=(int)(((double)step[i])*(sqrt((double)N)+M-1)/(M*ROOT2));
  q[i]=(int)(((double)step[i])*(sqrt((double)N)-1)/(M*ROOT2));
 }

 /* All Vertices of the Starting Simplex */
 for(i=1;i<N;i++)
 {
  for(j=0;j<M;j++) simp[i][j]=simp[0][j]+q[j];
  simp[i][i-1]=simp[0][i-1]+p[i-1];
  err=sum_residual(simp[i]);
  if(err) return(err);
 }

 /* Preset */
 for(i=0;i<N;i++)
 {
  l[i] = 1;
  h[i] = 1;
 }

 order();

 niter=0;

 /* Iterate */
 do
 {
  /* Wish it were True */
  *done=1;
  niter++;

  /* Compute Centroid...Excluding the Worst */

  for(i=0;i<N;i++) center[i]=0;

  for(i=0;i<N;i++)
   if(i!=h[N-1])
    for (j=0;j<M;j++) center[j]+=simp[i][j];

   /* First Attempt to Reflect */
  for(i=0;i<N;i++)
  {
   center[i]/=M;
 /*  next[i]=(1.0+ALPHA)*center[i] - ALPHA*simp[h[N-1]][i];  */

   next[i]=2*center[i] - simp[h[N-1]][i];
  }

  err=sum_residual(next);
  if(err) return(err);

  if(next[N-1]<=simp[l[N-1]][N-1])
  {
   new_vertex();
 /*  for(i=0;i<M;i++) next[i]=GAMMA*simp[h[N-1]][i]+(1.0-GAMMA)*center[i]; */

   for(i=0;i<M;i++) next[i]=2*simp[h[N-1]][i]-center[i];

   err=sum_residual(next);
   if(err) return(err);

   if(next[N-1]<=simp[l[N-1]][N-1]) new_vertex();
  }
  else
  {
   if(next[N-1]<=simp[h[N-1]][N-1]) new_vertex();
   else
   {
  /*  for(i=0;i<M;i++) next[i]=BETA*simp[h[N-1]][i]+(1.0-BETA)*center[i];  */


    for(i=0;i<M;i++) next[i]=(simp[h[N-1]][i]+center[i])>>1;


    err=sum_residual(next);
    if(err) return(err);

    if(next[N-1]<=simp[h[N-1]][N-1]) new_vertex();
    else
    {
     for(i=0;i<N;i++)
     {
   /*   for(j=0;j<M;j++) simp[i][j]=BETA*(simp[i][j]+simp[l[N-1]][j]);  */
      for(j=0;j<M;j++) simp[i][j]=(simp[i][j]+simp[l[N-1]][j])>>1;

      err=sum_residual(simp[i]);
      if(err) return(err);
     }
    }
   }
  }

  order();

  /* Check For Convergence */
  for(j=0;j<M;j++)
  {
   if(*done)
   {
  /*  dprintf(4,"h=%d l=%d",simp[h[j]][j],simp[l[j]][j]);  */

    if((simp[h[j]][j]-simp[l[j]][j])>(simp[h[j]][j]/512)) *done=0;
   }


/*   error[j]=(simp[h[j]][j]-simp[l[j]][j])/simp[h[j]][j];
   if(*done) 
    if(error[j]>maxerr[j]) *done=0;  */

  }
 } until((*done) || (niter == maxiter));


/* dprintf(1,"done=%d niter=%d",done,niter);
 dprintf(2,"p1=%d p2=%d",simp[0][0],simp[0][1]); */



 for(i=0;i<N;i++)
 {
  mean[i]=0;
  for(j=0;j<N;j++) mean[i]+=simp[j][i];
  mean[i]/=N;
 }

 fxoutput(mean[0],mean[1],mean[2],mean[3]);

 return(err);
}

