///////////////////////////////////////////////////////////////////////
//                                                                   //
//   KRLS 1.0                                                        //
//                                                                   //
//   Authors: Yaakov Engel & Shie Mannor                             //
//   Date: 24.11.2003                                                //
//                                                                   //
//   Copyright (c) 2003  Yaakov Engel & Shie Mannor                  //
//                                                                   //
//   This program 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.             //  
//                                                                   // 
//   This program 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 this program; if not, write to the Free      //
//   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139,   //
//   USA.                                                            //
//                                                                   //
///////////////////////////////////////////////////////////////////////

// KRLS with doubles

#include "stdio.h"
#include "stdlib.h"

#include <iostream>

#include <fstream>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <cstdlib>

#include <stdio.h>
#include <limits.h>

#define SQR(x) (x)*(x)
#define MAX_M 3000 // max dictionary size
#define MAX_D 1000 // max input dimension
#define WARN_THRESH 1e8 // unreliable results warning threshold
  
double nu,expmult,lambda,lambda2;
double kparam[3];
int kernel_type;
double kt[MAX_M];
double a[MAX_M];
double delta;
double Kinv[MAX_M][MAX_M];
double P[MAX_M][MAX_M];
double Pa[MAX_M], q[MAX_M];
double aPa;
double alpha[MAX_M];

int warn_flag=0;
int d,yindex,m;
long ell;
  
// functions

inline double kernel(double x1[MAX_D], double x2[MAX_D])
{ 
  int i;
  double k=0.0;
  switch(kernel_type)
    {
    case 0: // linear kernel
      for (i=0; i<yindex ; i++)
	k += x1[i]*x2[i];
      break;
      
    case 1: // polynomial kernel
      for (i=0; i<yindex; i++)
	k += x1[i]*x2[i];
      k = pow(kparam[1]*k+kparam[2], kparam[0]);
      break;
      
    case 2: // Gaussian kernel 
      for (i=0; i<yindex; i++)
	k = k + SQR(x1[i] - x2[i]);
      k = exp(expmult * k);
      break;
      
    default: // unknown
      fprintf(stderr,"krls: unknown kernel type: %d \n", kernel_type); 
      exit(-1);
      break;
    }		
  
  return (k);
}

inline double innerprod_m(double v1[MAX_M], double v2[MAX_M])
{
  int j;
  double dtmp;
  dtmp =0;
  for (j=0; j<m; j++)
    dtmp = dtmp + v1[j]*v2[j];
  return (dtmp);
}

//************************************************************
// MAIN 
//************************************************************
int main(int argc, char **argv)
{

  
  FILE * f_inp ;
  FILE * f_out ;
  // X-Y file name, nu, kparam, lambda
  char line[1024];
  int i,j,flag;

  //************************************************************
  // read arguments - options style
  //************************************************************
  if (argc < 2) { // needs at least 1 argument - the data file
    fprintf(stderr,"Usage:  krls <data_file_name> [-n <nu>] [-l <lambda>] [-k <kernel_type> [<kernel parameters>]] \n");
    exit(-1);
  }
  // define default parameter values
  nu=0.01;
  lambda=0.1;
  kernel_type=2; // Gaussian
  kparam[0]=1; // Gaussian: width (sigma), Polynomial: polynom degree
  kparam[1]=1; // Polynomial: constant multiplying inner product  
  kparam[2]=1; // Polynomial: constant added to inner product 
  
  int c;
  char *fname =*++argv;
  --argc;

  while (--argc>0 && (*++argv)[0]=='-')
    {
      c=*++argv[0];
      if(c=='n')
	{
	  sscanf(*++argv, "%lf", &nu);
	  --argc;
	}
      else if (c=='l')
	{
	  sscanf(*++argv, "%lf", &lambda);
	  --argc;
	}
      else if (c=='k')
	{
	  sscanf(*++argv, "%d", &kernel_type);
	  --argc;
	  for (i=0; i<3; i++)
	    {
	      if (argc-1==0 || *(argv+1)[0]=='-')
		break;
	      --argc;
	      ++argv;
	      sscanf(*argv, "%lf", &kparam[i]);
	    }
	}
      else
	{
	  fprintf(stderr,"krls: illegal option %c\n",c);
	  exit(-1);
	}
    }
  // fprintf(stderr,"Data file = %s nu = %lf lambda = %lf kernel_type = %d kernel_param = %lf %lf %lf\n",fname,nu,lambda,kernel_type,kparam[0],kparam[1],kparam[2]);
  
  //************************************************************
  // helper parameters
  lambda2= SQR(lambda);
  expmult = -1 / (2*SQR(kparam[0]));
  
  // Scan input data file

  f_inp = fopen(fname,"r");
  if (f_inp==NULL)
    {
    fprintf(stderr,"krls: Can't open file %s\n",fname);
    exit(-1);
    }

  fscanf(f_inp,"%ld %d",&ell,&d);
  if (d>=MAX_D) {
    fprintf(stderr,"krls: d = %d is too large\n",d);
    exit(-1);
  }
  yindex = d-1;
  double x[yindex], y;
  // fprintf(stderr,"Yaki 2\n");

  double dictionary[MAX_M][yindex];

  for (i=0; i<yindex; i++) {
    flag = fscanf(f_inp,"%lf",&x[i]);
    if (flag != 1) {
      fprintf(stderr,"krls: IO %d %d  \n",1,i);
      exit(-1);		
    }
  }
  flag = fscanf(f_inp,"%lf",&y);
  if (flag != 1) {
    fprintf(stderr,"krls: IO %d %d  \n",1,i);
    exit(-1);		
  }

  // fclose(f_inp);
  
  double ktt;									
  int ii[MAX_M];


  //  ktt=1+lambda^2; % k(x,x) for all x
  ktt = kernel(x,x) + lambda2;

  // % initialization
  // ii=[1];
  // m=1;

  for (i=0; i<yindex; i++)
    dictionary[0][i]=x[i];

  m=1;
  ii[0]=0;

  // kt=[];

  double alpha_mult;

  // Kinv=[1/ktt];
  Kinv[0][0] = 1/ktt;
  // a=[1];
  a[0]=1;
  delta=ktt;
  // P=[1]; % (At*A)^-1
  P[0][0] = 1;
  //  alpha=[Y(1)/delta];
  alpha[0] = y/delta;

  // ************ time loop *************
  int t;
  int j1,j2;
  // for t=2:l
  for (t=1; t< ell; t++) {  
    //  y=Y(t);
    for (i=0; i< d-1; i++) {
      flag = fscanf(f_inp,"%lf",&x[i]);
      if (flag != 1) {
	fprintf(stderr,"krls: IO %d %d  \n",j,i);
	exit(-1);		
      }
    }
    flag = fscanf(f_inp,"%lf",&y);
    if (flag != 1) {
      fprintf(stderr,"krls: IO %d %d  \n",j,i);
      exit(-1);		
    }	
    ktt = kernel(x,x) + lambda2;
    // fprintf(stderr,"ktt = %f  \n",ktt);
    // kt=kernel_row2(X(t,:),X(ii,:),kernel_param,kernel_str)'+lambda^2;
    for (j=0; j<m; j++) {
      kt[j] = kernel(x,dictionary[j]) + lambda2;
    } 
    // a=Kinv*kt;
    double norm_a=0;
    for (j=0; j<m; j++) {
      a[j] = innerprod_m(&Kinv[j][0], kt);
      norm_a +=SQR(a[j]);
    }
    //fprintf(stderr,"norm(a)^2 = %lf\n",norm_a);
    if (norm_a>WARN_THRESH)
      warn_flag=1;
    // d=y-kt'*alpha;
    double diffyf = y - innerprod_m(kt, alpha);
    // delta=ktt-kt'*a;
    delta = ktt - innerprod_m(kt, a);
    //fprintf(stderr,"C: t = %d, delta = %f \n",t+1,delta);
    //fprintf(stderr,"ktt = %f \n",ktt);
    // if delta > nu % k(x_t,.) is linearly independent of K
    if (delta > nu && m<MAX_M) {
      // ii=[ii,t];
      for (i=0; i < yindex; i++)
	dictionary[m][i]= x[i];
      ii[m]=t;
 
      // % update Kinv (iteratively using the previous Kinv)
      // Kinv=[delta*Kinv+a*a', -a; -a', 1]/delta;
      for (j1=0; j1<m; j1++) {
	for (j2=0; j2<=j1; j2++) {
	  Kinv[j1][j2] = Kinv[j1][j2]+a[j1]*a[j2]/delta;
	  Kinv[j2][j1] = Kinv[j1][j2];
	}
	Kinv[m][j1]=-a[j1]/delta;
	Kinv[j1][m]=Kinv[m][j1];
      }
      Kinv[m][m]=1/delta;
      // P=[P,z;z',1];
      for (j=0; j<m; j++) {
	P[m][j] = 0;
	P[j][m] = 0;
      }
      P[m][m] = 1;
      // old_alpha=alpha;
      // alpha=[old_alpha-d*a/delta];
      // alpha=[alpha;d/delta];
      alpha_mult = diffyf/delta;
      for (j=0; j<m; j++) {
	alpha[j] -= (a[j]*alpha_mult);
      }
      alpha[m]=alpha_mult;
      // a=[z;1];
      for (j=0; j<m; j++) {
	a[j]=0;
      }
      a[m]=1;
      // m=m+1;
      m++;
    }
    else { //% k(x_t,.) is linearly dependent on K
      // Pa=P*a; % P is symmetric
      for (j=0; j<m; j++) {
	Pa[j]=innerprod_m(&P[j][0], a);
      }
      for (j=0; j<m; j++) {
	aPa=innerprod_m(Pa, a);
      }
      // q=Pa/(1+a'*Pa);
      for (j=0; j<m; j++) {
	q[j]=Pa[j]/(1+aPa);
      }
      // P=P-q*Pa';
      for (j1=0; j1<m; j1++) {
	for (j2=0; j2<=j1; j2++) {
	  P[j1][j2]=P[j1][j2]-q[j1]*Pa[j2];
	  P[j2][j1]=P[j1][j2];
	}
      }
      // alpha=alpha+Kinv*q*d;
      for (j=0; j<m; j++) {
	alpha[j] += (innerprod_m(&Kinv[j][0], q) * diffyf);
      }
      // end % if delta
    }
    // end % of for t
    //	printf ("t = %d, alpha[0]=%lf\n",t,alpha[0]);
  }

  // OUTPUT to file

  sprintf (line, "%s.out",fname);
  f_out = fopen(line,"wt");

  fprintf(f_out,"%d\n",m);

  for (j=0;j<m;j++)
    fprintf(f_out,"%d ",ii[j]+1);
  fprintf(f_out,"\n");

  for (j=0;j<m;j++)
    fprintf(f_out,"%lf ",alpha[j]);
  fprintf(f_out,"\n");


  fclose(f_out);
  
  if (warn_flag)
    fprintf(stderr,"krls: Warning - results may be unreliable\n");

  return(0);
}