//#include <io.h>
//#include <system.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>


// square root of the sum of the squares (L2 norm)
double vec_norm(double *vector, int vec_size)
{
	double cur_element;
	double sum = 0;
	
	int i;
	for(i = 0; i < vec_size; i++)
	{
		cur_element = vector[i];
		sum += cur_element * cur_element;
	}
	
	return sqrt(sum);
}

// trans_mat = 1 if mat should be transposed, 0 otherwise
void mat_vec_mul(void **mat, double *vec, int mat_rows, int mat_cols, char trans_mat, double *result, char mat_is_int)
{
  int i,j;
  /*for( i = 0; i < mat_cols; i++ ) {
    for( j = 0; j < mat_rows; j++ ) 
      printf("mat[%i][%i] = %d\n",i,j, ((int **)mat)[i][j]);
  }*/

  if( !trans_mat ) {
 
    for( i = 0; i < mat_rows; i++ ) {
      for( j = 0; j < mat_cols; j++ ) 
      {
	result[i] += (((double **)mat)[i][j])*vec[j];
	//printf("%3.3f * %3.3f\n",(((double **)mat)[i][j]),vec[j]);
      }
    }
  
  }else {
    
     for( i = 0; i < mat_cols; i++ ) {
      for( j = 0; j < mat_rows; j++ )
      {
	result[i] += (((int **)mat)[j][i])*vec[j];
        //printf("%d * %3.3f\n",(((int **)mat)[j][i]),vec[j]);
      }
      
      //printf("result[%d] = %3.3f\n",i,result[i]);
     }
   
  }

  return;
}

//solves Ax = b for a UT cholesky decomposed matrix using backsubstitution
void linsolve(double **R_I, double *vec, int activeSetSize, double *result, char trans_mat)
{
  int i,k;
  double sum;

  if ( !trans_mat ) 
  {
    for(i = activeSetSize-1; i >= 0; i--)
    {
      for(sum = vec[i], k = i+1; k < activeSetSize; k++)
        sum -= R_I[i][k]*result[k];
	
      result[i] = sum/R_I[i][i];
      printf("result[%d] in linsolve loop1a = %3.3f / %3.3f\n",i,sum,R_I[i][i]);
    }

    for(i = 0; i < activeSetSize; i++)
    {
      for(sum = result[i], k = i-1; k >= 0; k--)
        sum -= R_I[i][k]*result[k];
	
      result[i] = sum/R_I[i][i];
      printf("result[%d] in linsolve loop2a = %3.3f / %3.3f\n",i,sum,R_I[i][i]);
    }
  }
  else {
    for(i = 0; i < activeSetSize; i++)
    {
      for(sum = vec[i], k = i-1; k >= 0; k--)
        sum -= R_I[i][k]*result[k];
	
      result[i] = sum/R_I[i][i];
      printf("result[%d] in linsolve loop1b = %3.3f / %3.3f\n",i,sum,R_I[i][i]);
    }
    
    for(i = activeSetSize-1; i >= 0; i--)
    {
      for(sum = result[i], k = i+1; k < activeSetSize; k++)
        sum -= R_I[i][k]*result[k];
	
      result[i] = sum/R_I[i][i];	
      printf("result[%d] in linsolve loop2b = %3.3f / %3.3f\n",i,sum,R_I[i][i]);
    }
  }

  return;
}

void updateChol(double **R, int n, int N, int **A, int *activeSet, int activeSetSize, int newIndex)
{
  int i;
  double sum,q;
  //double *newVec = malloc( sizeof(double)*activeSetSize );
  //double *result = malloc( sizeof(double)*activeSetSize );
  //double *p      = malloc( sizeof(double)*activeSetSize );
  
  double newVec[n];
  double result[activeSetSize];
  double p[activeSetSize];
  for( i = 0; i < n; i++ ) 
    newVec[i] = (double)A[i][newIndex];

  if( !activeSetSize ) 
  {
    for( i = 0; i < n; i++ )
    {
      sum += powf(newVec[i],2);
      printf("in updateChol, sum now equals %3.3f\n",sum);
    }
    
    R[0][0] = sqrt(sum);
  }
  else
  {
    mat_vec_mul( (void **)A, newVec, n, activeSetSize, 1, result, 1 );
    linsolve( R, result, activeSetSize, p, 1 );
    for( i = 0; i < activeSetSize; i++ )
    {
      q += newVec[i]*newVec[i] - p[i]*p[i];
      R[i][activeSetSize] = p[i];
    }
    
    R[activeSetSize][activeSetSize] = sqrt(q);
    printf("R[%d][%d] = %3.3f\n",activeSetSize,activeSetSize,sqrt(q));
  }
  
  for ( i = 0; i < activeSetSize; i++ ) 
    printf("R[%i][0] = %f\n",i,R[i][0]);
  
  return;
}
