#include <math.h>
#include <unistd.h>
#include "ideal.h"

void make_ideal_field(vec_field *field, //vector field to build 
		      float Tx, float Ty, float Tz, //Translational Para 
		      float Z, //Z component
		      float Omx, float Omy, float Omz, //Rotational Para
		      float error1, float error2, float error3, //Errors 
		      float *average, //Average error of MF(=Motion Field)
		      float *std_dev, //Standard deviation of M  
		      float *mag_average,
		      float *mag_std_dev, //Standard deviation of M
		      float *density, //Density of MF
		      float *outlier ) // Percentage of outlier
{
  float u,v;
  int x,y,i;
  int xCenter=field->width>>1;
  int yCenter=field->height>>1;
  int xnew,ynew;
  double error[field->datasize],sum,tmp,cosi;
  double mag_error[field->datasize],mag_sum;
  int anz=0,out=0;
  
  for(y=field->yoffset; 
      (unsigned) y < field->height-field->yoffset; y++)
    {
      for(x=field->xoffset; 
	  (unsigned)x < field->width-field->xoffset; x++)
	{
	  //Entry of MF
	  i=y * field->width + x ;
	  
	  //Center of Image
	  xnew=x-xCenter;
	  ynew=-y+yCenter;
	  
	  //Motion Field
	  u=(xnew-Tx/Tz)*Tz/Z
	    +xnew*ynew*Omx-(1+xnew*xnew)*Omy+ynew*Omz;
	  v=(ynew-Ty/Tz)*Tz/Z
	    +(1+ynew*ynew)*Omx-xnew*ynew*Omy-xnew*Omz;
	  
	  if(error1!=0.0)
	    {
	      //Add Random error to MF vectors
	      field->u[i]= u*(1+(2.0*uni()-1.0)/error1);
	      field->v[i]=-v*(1+(2.0*uni()-1.0)/error1);
	    }
	  else
	    {
	      field->u[i]= u;
	      field->v[i]=-v;
	    }
	  
	  //Produce sparse vector field
	  if( (int) (uni()*error2) == 1 && error2!=0.0)
	    field->valid[y * field->width + x ]=0;
	  else
	    {
	      field->valid[y * field->width + x ]=1;
	      
	      if(error1!=0.0)
		{
		  //Calculate error
		  tmp=( sqrt(u*u + v*v + 1)*
			sqrt(field->u[i]*field->u[i]
			     +field->v[i]*field->v[i] + 1) );
		  
		  if(tmp < 0.00001)
		    tmp=1e10;
		  else
		    tmp=1.0 / tmp;
		  
		  cosi=tmp*(u*field->u[i]-v*field->v[i]+1);
		  
		  if(cosi > 1.0)
		    cosi=1;
		  if(cosi < -1.0)
		    cosi=-1;
		  
		  error[anz]=acos(cosi);
		  
		  mag_error[anz++]=sqrt((u-field->u[i])*(u-field->u[i])+
					((v+field->v[i])*(v+field->v[i])))
		    /sqrt(u*u+v*v);
		}
	      else
		error[anz++]=0.0;
	      
	      //Add outlier to MF
	      if((int) ((uni())*error3)==1 && error3!=0.0)
		{
		  field->u[i]= ( u * (uni()*10-5) );
		  field->v[i]= (-v * (uni()*10-5) );
		  //anz--;
		  out++; 
		}
	    }
	  
	}
    }
  anz--;
   
  //Build Statistics
  //Calculate average error
  sum=0.0;mag_sum=0.0;
  for(i=0; i < anz; i++)
    {
      //sum+=((error[i]>=0.0)?(error[i]):(6.28+error[i]));
      sum+=error[i];
      mag_sum+=mag_error[i];
    }
  
  //Convert from radians into degree
  *average = (sum / (float) anz);
  *mag_average = (mag_sum / (float) anz);

  //Calculate standard deviation
  sum=0.0;mag_sum=0.0;
  for(i=0; i < anz; i++)
    {
      sum+=(error[i]-(*average))*
	(error[i]-(*average));
      
      mag_sum+=(mag_error[i]-(*mag_average))*(mag_error[i]-(*mag_average));
      
    }
  
  *average = (*average)*(180.0 / 3.14);

  *std_dev = sum / (float) (anz-1)*(180.0 / 3.14)*(180.0 / 3.14);
  
  *mag_std_dev = mag_sum / (float) (anz-1);
  
  *density=(float) anz/field->datasize*100;
  
  *outlier=(float) out/anz*100;
  
  //fprintf(stdout,"Average Error: %f Std_dev: %f Density:%f  Outlier:%f\n",
  //  *average, *std_dev, *density, *outlier);
  
  //fprintf(stdout,"Average Magnitude Error: %f Std_dev: %f\n",
  //  *mag_average, *mag_std_dev);
  
}


double uni()
{
  static long s1 = 55555; 
  static long s2 = 99999;
  static double factor = 1.0/2147483563.0;
  register long k,z;
  
  s1=getpid()*100;
 
  k= s1 /53668;
  s1 =40014*(s1%53668)-k*12211;
  if (s1 < 0) 
    s1 += 2147483563;
  
  k=s2/52774;
  s2=40692*(s2%52774)-k*3791;
  
  if (s2 < 0) 
    s2 += 2147483399;
  
  z= (s1 - 2147483563) + s2;
  if (z < 1) z += 2147483562;
  
  return(((double)(z))*factor);
}


