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

/* this is the trigger for stopping the entire algorithm.  decreasing it
should directly improve the minima found.  */

/* #define OUT_TOLERANCE 0.00001 */

/* ignore */
#define ITER_MAX 100

/* defines the tolerance for deciding when to stop the conjugate gradient
descent process */

/* #define ZERO_TOLERANCE1 0.0001 */

/* defines the tolerance for deciding whether an alpha is actually zero or 
not when the algorithm terminates */

/* #define ZERO_TOLERANCE2 0.1 */

/* ignore */
#define ZERO_TOLERANCE3 0.0001
#define ZERO_TOLERANCE4 10e-9
#define ZERO_SLOP 0.001
#define DX 0.001

/* sets the number of input examples */

/* #define dim 250 */

/* sets the dimension of the examples */

/* #define INPUT_DIMENSION 2 */

/* ignore */
#define OUTRAGEOUS 100000000
#define SEP_CON 0.001

int INPUT_DIMENSION;
int M;
int N;

double OUT_TOLERANCE, ZERO_TOLERANCE1, ZERO_TOLERANCE2;

double box_constraints1,box_constraints0;
double ***input_x;
int **input_y;

double *func_storage;
double *old_point;
int active_directions;

int dim;
int ABcut;

#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}

void invert(double **a, int n)
{
  int *indxc, *indxr, *ipiv;
  int i, icol, irow, j, k, l, ll;
  double big,dum,pivinv,temp;

  indxc = (int *) calloc (sizeof(int), n);
  indxr = (int *) calloc (sizeof(int), n);
  ipiv = (int *) calloc (sizeof(int), n);
  for (j=0; j<n; j++) ipiv[j]=0;
  for (i=0;i<n;i++) {
    big=0.0;
    for (j=0;j<n;j++)
      if (ipiv[j] != 1)
	for (k=0;k<n;k++) {
	  if (ipiv[k] == 0) {
	    if (fabs(a[j][k]) >= big) {
	      big = fabs(a[j][k]);
	      irow = j;
	      icol = k;
	    }
	  } else if (ipiv[k] > 1) { printf("Singular matrix.  Exit 1.\n"); exit();}
	}
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l=0;l<n;l++) 
	SWAP(a[irow][l],a[icol][l])
	}
    indxr[i]=irow;
    indxc[i]=icol;
    if (a[icol][icol] == 0.0) {printf("Singular matrix.  Exit 2.\n"); exit();}
    pivinv = 1.0/a[icol][icol];
    a[icol][icol]=1.0;
    for (l=0;l<n;l++) a[icol][l] *= pivinv;
    for (ll=0;ll<n;ll++)
      if (ll != icol) {
	dum=a[ll][icol];
	a[ll][icol]=0.0;
	for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
      }
  }
  for (l=(n-1);l>=0;l--) {
    if (indxr[l] != indxc[l])
      for (k=0;k<n;k++)
	SWAP(a[k][indxr[l]],a[k][indxc[l]]);
  }
  free(ipiv);
  free(indxr);
  free(indxc);
}
  



double dot_product(double x[],double y[])
{
double out=0;
int foo;
for (foo = 0; foo < ABcut; ++foo)
  out += x[foo] * y[foo];
return out;
}

double mylog (double x)
{
  return (log(x));
}

double func(double point[])
{
  int i,j;
  double accumulation=0;
  double *alpha;
  double *beta;

  alpha = (double *) calloc (sizeof(double), ABcut);
  beta = (double *) calloc (sizeof(double), ABcut);
  for (i = 0; i < ABcut; ++i)
    {
      alpha[i] = point[i];
      beta[i] = point[i+ABcut];
    }
  
  for (i = 0; i < M; ++i)
    {
      for (j = 1; j < N; ++j)
	{
	  accumulation += mylog(1 + exp(-dot_product(input_x[i][j],beta) + (input_y[i][j-1]*dot_product(input_x[i][j],alpha))));
	}
    }
  free(alpha);
  free(beta);
  return(accumulation);
}

void dfunc(double point[], double direction[])
{
  int foo,i,j;
  double accumulation;
  double logtemp;
  double *alpha;
  double *beta;

  alpha = (double *) calloc (sizeof(double), ABcut);
  beta = (double *) calloc (sizeof(double), ABcut);
  for (i = 0; i < ABcut; ++i)
    {
      alpha[i] = point[i];
      beta[i] = point[i+ABcut];
    }
  
  for (foo = 0; foo < dim; ++foo)
    {
      accumulation = 0;
      for (i = 0; i < M; ++i)
	{
	  for (j = 1; j < N; ++j)
	    {
	      logtemp = mylog(1 + exp(-dot_product(input_x[i][j],beta) + (input_y[i][j-1]*dot_product(input_x[i][j],alpha))));
	      if (foo < ABcut)
		accumulation += (input_y[i][j-1]*input_x[i][j][foo]*logtemp) / (1 + logtemp);
	      else
		accumulation += (-input_x[i][j][foo-ABcut]*logtemp) / (1 + logtemp);
	    }
	}
      direction[foo] = accumulation;
    }
  free(alpha);
  free(beta);
}

double sqr (double x)
{
return (x * x);
}

double **covariance;

void ddfunc(double point[])
{
  int foo,bar,i,j;
  double accumulation;
  double logtemp;
  double temp;
  double *alpha;
  double *beta;

  alpha = (double *) calloc (sizeof(double), ABcut);
  beta = (double *) calloc (sizeof(double), ABcut);
  for (i = 0; i < ABcut; ++i)
    {
      alpha[i] = point[i];
      beta[i] = point[i+ABcut];
    }
  
  for (foo = 0; foo < dim; ++foo)
    {
      for (bar = 0; bar < dim; ++bar)
	{
	  accumulation = 0;
	  for (i = 0; i < M; ++i)
	    {
	      for (j = 1; j < N; ++j)
		{
		  logtemp = mylog(1 + exp(-dot_product(input_x[i][j],beta) + (input_y[i][j-1]*dot_product(input_x[i][j],alpha))));
		  logtemp = logtemp / (1 + logtemp);
		  if (foo < ABcut)
		    {
		      if (bar < ABcut)
			{
			  temp = (-input_x[i][j][foo]*input_x[i][j][bar])*sqr(input_y[i][j-1]);
			  accumulation += (temp * logtemp) - (temp * sqr(logtemp));
			}
		      else 
			{
			  temp = (-input_x[i][j][foo]*input_x[i][j][bar-ABcut])*input_y[i][j-1];
			  accumulation += (temp*sqr(logtemp)) - (temp*logtemp);
			}
		    }
		  else
		    {
		      if (bar < ABcut)
			{
			  temp = (-input_x[i][j][foo-ABcut]*input_x[i][j][bar])*input_y[i][j-1];
			  accumulation += (temp*sqr(logtemp)) - (temp*logtemp);
			}
		      else
			{
			  temp = (-input_x[i][j][foo-ABcut]*input_x[i][j][bar-ABcut]);
			  accumulation += (temp*logtemp) - (temp*(sqr(logtemp)));
			}
			
		    }
		}
	    }
	  covariance[foo][bar] = accumulation;
	}
    }
  free(alpha);
  free(beta);
}

void project(int projected_onto_vector[], double projected_vector[])
{
int foo, bar;
double ab, aa, moses;
aa = ab = 0;
for (foo = 0; foo < dim; foo++)
  {
    aa += projected_onto_vector[foo] * projected_onto_vector[foo];
    ab += projected_onto_vector[foo] * projected_vector[foo];
  }

moses = ab / aa;
for (foo = 0; foo < dim; foo++)
  projected_vector[foo] -= projected_onto_vector[foo] * moses;
}


void linmin(double init_point[], double direction[], double *min_value)
{
  int cd;
  int foo;
  double poss_min;
  double p_a, p_b, p_c;
  double p_f_a, p_f_b, p_f_c;
  double virtual_dx,neg_virtual_dx,posi_virtual_dx, poss_virtual_dx;
  double local_dx;
  double *local_direction;
  int *local_hyperplane;
  int flag;
  double b, *maximal_extension, *step_back, f_at_bounds, f_back;
  int protrusion;
  double measure, prev_measure;
  double previous_value, current_value, *previous_point, diff;
  
  local_direction = (double *) calloc (sizeof(double), dim);
  local_hyperplane = (int *) calloc (sizeof(double), dim);
  maximal_extension = (double *) calloc (sizeof(double), dim);
  step_back = (double *) calloc (sizeof(double), dim);
  previous_point = (double *) calloc (sizeof(double), dim);

  local_dx = DX;
  diff = 1;
  active_directions = 250;
  for (foo = 0; foo < dim; foo++)
    {
      local_direction[foo] = direction[foo];
      if (local_direction[foo] == 0)
	--active_directions;
      /* local_hyperplane[foo] = input_y[foo]; */
    }
  prev_measure = OUTRAGEOUS;
  current_value = *min_value;
  cd = 1;

  while (diff > 0)
    {
      if (cd > 0) /* if we've hit a boundary or are just starting */
	{
	  /* first check if the min of the new direction is at the boundary */
	  prev_measure = OUTRAGEOUS;
	  for (foo = 0; foo < dim; ++foo)
	    {
	      b = -1;
	      if (local_direction[foo] < 0)
		b = (box_constraints0 - init_point[foo]) / local_direction[foo];
	      else if (local_direction[foo] > 0)
		b = (box_constraints1 - init_point[foo]) / local_direction[foo];
	      if ((b >= 0) && (b < prev_measure))
		    {
		      prev_measure = b;
		      protrusion = foo;
		    }
	    }
	  for (foo = 0; foo < dim; ++foo)
	    {
	      maximal_extension[foo] = init_point[foo] + (local_direction[foo] * prev_measure);
	      step_back[foo] = maximal_extension[foo] - (SEP_CON * local_direction[foo]);
	    }
	  /* step_back to ensure we haven't found something better but not
	     the minimum */

	  f_at_bounds = func(maximal_extension);
	  
	  if (f_at_bounds < current_value)
	    {
	      if (f_at_bounds < func(step_back))
		{
		  for (foo = 0; foo < dim; ++foo)
		    init_point[foo] = maximal_extension[foo];
		  current_value = f_at_bounds;
		}
	    }
	  /* otherwise, solve for the parabola */
	  if (current_value != f_at_bounds)
	    {
	      p_a = 0;
	      p_c = prev_measure;
	      p_b = p_c / 2;
	      p_f_a = current_value;
	      p_f_c = f_at_bounds;
	      for (foo = 0; foo < dim; ++foo)
		maximal_extension[foo] = init_point[foo] + (local_direction[foo] * p_b);
	      p_f_b = func(maximal_extension);
	      f_back = (((p_b-p_a)*(p_f_b-p_f_c))-(((p_b-p_c)*(p_f_b-p_f_a))));
	      if (f_back != 0)
		{
		  poss_min = p_b - (0.5 * (((p_b - p_a)*(p_b - p_a)*(p_f_b-p_f_c)) - ((p_b-p_c)*(p_b-p_c)*(p_f_b-p_f_a))) / f_back);
		  for (foo = 0; foo < dim; ++foo)
		    maximal_extension[foo] = init_point[foo] + (local_direction[foo] * poss_min);
		  f_back = func(maximal_extension);
		  if (f_back < current_value)
		    {
		      for (foo = 0; foo < dim; ++foo)
			init_point[foo] = maximal_extension[foo];
		      current_value = f_back;
		      
		      *min_value = current_value;
		      free(local_direction);
		      free(local_hyperplane);
		      free(maximal_extension);
		      free(step_back);
		      free(previous_point);
		      return;
		      
		    }
		}
	    }
	  cd = 0;
	}
      previous_value = current_value;
      posi_virtual_dx = OUTRAGEOUS;
      neg_virtual_dx = -OUTRAGEOUS;
      /* hop forward perhaps a step to clean up parabolic interpolation */
      for (foo = 0; foo < dim; foo++)
	{
	  previous_point[foo] = init_point[foo];
	  init_point[foo] += (local_dx * local_direction[foo]);
	  if (init_point[foo] < box_constraints0)
	    {
	      if (local_direction[foo] != 0)
		poss_virtual_dx = (previous_point[foo]-box_constraints0)/local_direction[foo];
	      if (poss_virtual_dx > neg_virtual_dx)
		neg_virtual_dx = poss_virtual_dx;
	      local_direction[foo] = 0;
	      --active_directions;
	      local_hyperplane[foo] = 0;
	      cd = 1;
	    }
	  if (init_point[foo] > box_constraints1)
	    {
	      if (local_direction[foo] != 0)
		poss_virtual_dx = (box_constraints1-previous_point[foo])/local_direction[foo];
	      if (poss_virtual_dx < posi_virtual_dx)
		posi_virtual_dx = poss_virtual_dx;
	      cd = 2;
	      local_direction[foo] = 0;
	      --active_directions;
	      local_hyperplane[foo] = 0;
	    }
	}

      /* if we've banged into a wall, correct the direction vector */
      if (cd > 0)
	{
	  if (cd == 1)
	    virtual_dx = neg_virtual_dx;
	  if (cd == 2)
	    virtual_dx = posi_virtual_dx;
	  for (foo = 0; foo < dim; ++foo)
	    init_point[foo] = previous_point[foo] + (local_direction[foo] * virtual_dx) ;
	  /*project(local_hyperplane, local_direction);*/
	  if (virtual_dx > ZERO_TOLERANCE3)
	    {
	      current_value = func(init_point);
	      diff = previous_value - current_value;
	    }
	}
      else
	{
	  current_value = func(init_point);
	  diff = previous_value - current_value;
	}
    }
  for (foo = 0; foo < dim; foo++)
    init_point[foo] = previous_point[foo];
  *min_value = previous_value;
  free(local_direction);
  free(local_hyperplane);
  free(maximal_extension);
  free(step_back);
  free(previous_point);
}

int find_min (double init_point[], double tolerance, int *iterations, double *min_value)
{
  int foo, iters;
  double gg, gam, f_of_init_point, dgg;
  double *g, *h, *x_i;
  void linmin(), free_vector();

  g = (double *) calloc (sizeof(double), dim);
  h = (double *) calloc (sizeof(double), dim);
  x_i = (double *) calloc (sizeof(double), dim);
  
  f_of_init_point = *min_value;
  dfunc(init_point, x_i);
  /*  project(input_y,x_i); */
  for (foo = 0; foo < dim; foo++)
    {
      g[foo] = -x_i[foo];
      x_i[foo] = h[foo] = g[foo];
    }
  for (iters = 1; iters <= ITER_MAX; iters++)
    {
      *iterations = iters;
      linmin(init_point, x_i, min_value);
      if (2.0 * fabs((*min_value - f_of_init_point)) <= tolerance * (fabs(*min_value) + fabs(f_of_init_point) + ZERO_SLOP))
	{
	  free(g);
	  free(h);
	  free(x_i);
	  return;
	}
      f_of_init_point = *min_value;
      dfunc(init_point, x_i);
      /* project(input_y, x_i); */
      dgg = gg = 0.0;
      for (foo = 0; foo < dim; foo++)
	{
	  gg += g[foo] * g[foo];
	  dgg += x_i[foo] * x_i[foo];
	}
      if (gg == 0.0)
	{
	  free(g);
	  free(h);
	  free(x_i);
	  return;
	}
      gam = dgg / gg;
      for (foo = 0; foo < dim; foo++)
	{
	  g[foo] = -x_i[foo];
	  x_i[foo] = h[foo] = g[foo] + gam * h[foo];
	}
    }
  fprintf(stderr,"Too many iterations.\n");
}

main()
{
  double min;
  double old_min;
  int foo,bar,quux;
  int its;
  double *init_point;
  int TOTAL_LENGTH;
  int alpha_count=0;
  int iters=1;

  FILE *fp;
  FILE *op;
  
  fp = fopen("mle-3.dat","r");
  op = fopen("mle.out","w");

  fscanf(fp,"%d",&ABcut);
  dim = ABcut * 2;
  fscanf(fp,"%d",&M);
  fscanf(fp,"%d",&N);
  fscanf(fp,"%lf",&OUT_TOLERANCE);
  fscanf(fp,"%lf",&ZERO_TOLERANCE1);
  fscanf(fp,"%lf",&ZERO_TOLERANCE2);
/*
  fscanf(fp,"%lf",&box_constraints0);
  fscanf(fp,"%lf",&box_constraints1);
*/
box_constraints0 = 0;
box_constraints1 = 50;

  func_storage = (double *) calloc (sizeof (double),dim );
  old_point = (double *) calloc (sizeof (double), dim);

  input_x = (double * * *) calloc (sizeof (double * *), M);
  input_y = (int * *) calloc (sizeof (int *), M);
  for (foo = 0; foo < M; ++foo)
    {
      input_x[foo] = (double * *) calloc (sizeof (double *), N);
      input_y[foo] = (int *) calloc (sizeof (int), N);
    }

  for (foo = 0; foo < M; ++foo)
    for (bar = 0; bar < N; ++bar)
      input_x[foo][bar] = (double *) calloc (sizeof (double), ABcut);
 
  init_point = (double *) calloc (sizeof (double), dim);
  covariance = (double * *) calloc (sizeof (double *), dim);
  for (foo = 0; foo < dim; ++foo)
    covariance[foo] = (double *) calloc (sizeof (double), dim);
  
  for (foo = 0; foo < ABcut; ++foo)
    for (bar = 0; bar < ABcut; ++bar)
      covariance[foo][bar] = 1;

  min = 0;
  for (foo = 0; foo < M; ++foo)
    for (bar = 1; bar < N; ++bar)
      for (quux = 0; quux < ABcut; ++quux)
	{
	  fscanf(fp,"%lf",&input_x[foo][bar][quux]);
	/*  printf("%d %d %d : %lf\n",foo,bar,quux,input_x[foo][bar][quux]); */
	  min += input_x[foo][bar][quux];
	}
  printf("Min : %lf\n",min);
  quux = 0;
  for (foo = 0; foo < M; ++foo)
    for (bar = 0; bar < (N-1); ++bar)
      {
	fscanf(fp,"%d",&input_y[foo][bar]);
	quux += input_y[foo][bar];
      }
  printf("Quux : %d\n",quux);

  for (foo = 0; foo < dim; ++foo)
    init_point[foo] = 0;
  
  printf("%lf\n",func(init_point));
  printf("Starting!\n");
  
  its = 0;
  min = func(init_point);
  old_min = min + 1000;
  
  while (old_min - min > OUT_TOLERANCE)
    {
      old_min = min;
      printf("%lf\n",min);
      find_min (init_point, ZERO_TOLERANCE1, &its, &min);
    }

  ddfunc(init_point);
  for (foo = 0; foo < dim; ++foo)
    {
      for (bar = 0; bar < dim; ++bar)
	printf("%lf ",covariance[foo][bar]);
      printf("\n");
    }
  invert(covariance,ABcut);
  printf("Finishing!\n");
  for (foo = 0; foo < dim; ++foo)
    fprintf(op,"%lf\n",init_point[foo]);
  fprintf(op,"\n \n");
  for (foo = 0; foo < ABcut; ++foo)
    fprintf(op,"%lf\n",sqrt(covariance[foo][foo]));
}