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

void print_matrix(char mat_name[], void **mat, int rows, int cols, char mat_is_int, char in_line)
{
	int i,j;
	
	if (in_line == 0)
	{
		for (i = 0; i < rows; i++)
		{
			for (j = 0; j < cols; j++)
			{
				printf("%s[%d][%d] = %d\n",mat_name,i,j,((int **)mat)[i][j]);
			}
		}
	}
	else
	{
		for (i = 0; i < rows; i++)
		{
			printf("%s[%d] = {",mat_name,i);
			
			for (j = 0; j < cols; j++)
			{
				printf("%d, \n",((int **)mat)[i][j]);
			}
			
			printf("};\n\n");
		}
	}
}

void print_vector(char vec_name[], void *vec, int rows, char vec_is_int, char in_line)
{
	int i;
	
	if (in_line == 0)
	{
		for (i = 0; i < rows; i++)
		{
			printf("%s[%d] = %d\n",vec_name,i,((int *)vec)[i]);
		}
	}
	else
	{
		printf("%s = {",vec_name);
	
		for (i = 0; i < rows; i++)
		{
			printf("%d, ",((int *)vec)[i]);
		}
		
		printf("};\n");
	}
}

// square root of the sum of the squares (L2 norm)
int vec_norm(int *vector, int vec_size)
{
	long int cur_element;
	long long int sum = 0;
	long int product = 0;
	
	int i;
	
	//printf("vec_norm\n");
	
	for(i = 0; i < vec_size; i++)
	{
		cur_element = vector[i];
		//printf("%d: %d\n",i,cur_element);
		product = cur_element * cur_element;
		//printf("product: %ld\n",product);
		sum += product;
		//printf("sum: %lld\n",sum);
	}
	
	//printf("sum: %lld\n",sum);
	
	//printf("vec_norm: %d\n",(int)sqrt(sum));
	return (int)sqrt(sum);
}

// trans_mat = 1 if mat should be transposed, 0 otherwise
void mat_vec_mul(int **mat, int *vec, int mat_rows, int mat_cols, char trans_mat, int *result, char mat_is_int)
{
  printf("mat_vec_mul\n");
  //printf("trans_mat = %d\n",trans_mat);
  int i,j;
  
  //printf("pre-transpose: cols=%d, rows=%d\n",mat_cols,mat_rows);
  
  int running_sum = 0;

	for( i = mat_cols - 1; i >= 0; i-- )
	{
		running_sum = 0;
	
		for( j = mat_rows - 1; j >= 0; j-- )
			running_sum += mat[j][i]*vec[j];
			
		result[i] = running_sum;
	}
  return;
}

//solves Ax = b for a UT cholesky decomposed matrix using backsubstitution
void linsolve(int **R_I, int *vec, int activeSetSize, int *result, char trans_mat)
{
  //printf("linsolve: trans_mat = %d\n",trans_mat);
  int i,k;
  int 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 {
  
    //int **R_I_T = (int **)malloc(activeSetSize * sizeof(int *));
    
    int i, j;
    /*for (i = 0; i < activeSetSize; i++)
    {
    	R_I_T[i] = (int *)malloc(activeSetSize * sizeof(int));
	
    	for(j = 0; j < activeSetSize; j++)
	{
		R_I_T[i][j] = R_I[j][i];
	}
    }*/
  
     for(i = 0; i < activeSetSize; i++)
    {
      for(sum = vec[i], k = i-1; k >= 0; k--)
        sum -= R_I[k][i]*result[k];
	//sum -= R_I_T[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_T[i][k]*result[k];
	
      result[i] = sum/R_I_T[i][i];
      //      printf("result[%d] in linsolve loop2a = %3.3f / %3.3f\n",i,sum,R_I[i][i]);
    }*/
    
    /*for(i = 0; i < activeSetSize; i++)
    {
      printf("vec[i] = %f for i = %d\n", vec[i], 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]);
    }*/
    
    //for (i = 0; i < activeSetSize; i++)
    //	free(R_I_T[i]);
	
    //free(R_I_T);
  }

  return;
}

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

  //print_vector("newVec", newVec, n, 0, 1);
    
  if( !activeSetSize ) 
  {
    for( i = 0; i < n; i++ )
    {
      sum += (int)pow(newVec[i],2);
      //printf("in updateChol, sum now equals %3.3f\n",sum);
    }
    
    R[0][0] = (int)sqrt(sum);
    //printf("R[0][0] = %f\n",R[0][0]);
  }

  else
  {
    //mat_vec_mul( (void **)A, newVec, n, activeSetSize, 1, result, 1 );
    for ( i = 0; i < activeSetSize; i++ )
    {
      ix = activeSet[i];
      result[i] = 0;
      for ( j = 0; j < n; j++ )
	result[i] += A[j][ix]*newVec[j];
    }
    
    //print_vector("result in updateChol", result, activeSetSize, 0, 1);
    
    linsolve( R, result, activeSetSize, p, 1 );
    
    //print_vector("p", p, activeSetSize, 0, 1);
    
    for( i = 0; i < activeSetSize; i++ )
    {
      //      printf("p[%i] = %f\n",i,p[i]);
      q += p[i]*p[i];
      //      printf("q = %3.3f\n",q);
      R[i][activeSetSize] = p[i];
    }
    
    R[activeSetSize][activeSetSize] = (int)sqrt(newVecSqrSum - 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;
}
