//matrivec.cpp: vector and matrix manipulation routines for use in calibrate.cpp

vector::vector(int elements)
{
	values = new double[length = elements];
}

vector::vector(int elements, double fillVal)
{
	values = new double[length = elements];
	for(int i = 0; i < length; i++)
		values[i] = fillVal;
}

vector::vector(double* dbls, int elements)
{
	values = new double[length = elements];
	for(int i = 0; i < length; i++)
		values[i] = dbls[i];
}

const vector vector::operator = (const vector v)
{
	resize(length = v.length);
	for(int i = 0; i < length; i++)
		values[i] = v.values[i];
}

vector vector::operator + (vector v)
{
	if(length != v.length)
	{
		cout << "Error in vector::operator +: vector lengths are not equal (" << length << " and " << v.length << ")!\n";
		exit(0);
	}
	vector temp(length);
	for(int i = 0; i < length; i++)
		temp[i] = values[i] + v.values[i];
}

void vector::operator += (vector v)
{
	if(length != v.length)
	{
		cout << "Error in vector::operator +=: vector lengths are not equal (" << length << " and " << v.length << ")!\n";
		exit(0);
	}
	for(int i = 0; i < length; i++)
		values[i] += v.values[i];
}

vector vector::operator - (vector v)
{
	if(length != v.length)	
	{
		cout << "Error in vector::operator -: vector lengths are not equal (" << length << " and " << v.length << ")!\n";
		exit(0);
	}
	vector temp(length);
	for(int i = 0; i < length; i++)
		temp[i] = values[i] + v.values[i];
	return temp;
}

void vector::operator -= (vector v)
{
	if(length != v.length)	
	{
		cout << "Error in vector::operator -=: vector lengths are not equal (" << length << " and " << v.length << ")!\n";
		exit(0);
	}
	for(int i = 0; i < length; i++)
		values[i] -= v.values[i];
}

vector vector::operator * (double scalar)
{
	vector temp(length);
	for(int i = 0; i < length; i++)
		temp[i] = values[i] * scalar;
	return temp;
}

void vector::operator *= (double scalar)
{
	for(int i = 0; i < length; i++)
		values[i] *= scalar;
}

matrix vector::operator * (vector v)
{
	matrix temp;
	multiply(v, temp);
	return temp;
}

vector vector::operator * (matrix m)
{
	if(m.numRows != length || m.numCols != length)
	{
		cout << "Error in vector::operator * (matrix): vector size is " << length << " and matrix size is " << m.numRows << " x "
			  << m.numCols << "!\n";
		exit(0);
	}
	vector temp(length);
	multiply(m, temp);
	return temp;
}

const int vector::getLength()
{
	return length;
}

const double& vector::operator [] (int index) const
{
	return values[index];
}

double& vector::operator [] (int index)
{
	return values[index];
}

void vector::scale(double scalar)
{
	for(int i = 0; i < length; i++)
		values[i] *= scalar;
}

void vector::fill(double* dbls, int numElements)
{
	int minLen = min(length, numElements);
	for(int i = 0; i < minLen; i++)
		values[i] = dbls[i];
}

void vector::resize(int newLen)
{
	if(!length)
	{
		values = new double[newLen];
		length = newLen;
		return;
	}
	int minLen = min(length, newLen);
	double* newVals = new double[length = newLen];
	for(int i = 0; i < minLen; i++)
		newVals[i] = values[i];
	values = newVals;
}

double vector::norm()
{
	double sum = 0;
	for(int i = 0; i < length; i++)
		sum += values[i] * values[i];
	return sqrt(sum);
}

double vector::dotProduct(vector v)
{
	if(length != v.length)	
	{
		cout << "Error in vector::dotProduct(): vector lengths are unequal (" << length << " and " << v.length << ")!\n";
		exit(0);
	}
	double sum = 0;
	for(int i = 0; i < length; i++)
		sum += values[i] * v.values[i];	
	return sum;
}

vector vector::crossProduct(vector v)
{
	if(length != 3 || v.length != 3)
	{
		printf("Error in vector::crossProduct(): vector not of size 3!\n");
		exit(0);
	}
	vector result(3);
	result[0] = values[1] * v.values[2] - values[2] * v.values[1];
	result[1] = values[2] * v.values[0] - values[0] * v.values[2];
	result[2] = values[0] * v.values[1] - values[1] * v.values[0];
	return result;
}

void vector::multiply(matrix m, vector& result) //horiz vector x matrix --> horiz vector
{
	if(length != m.numRows)	
	{		
		printf("Error in vec mult by matrix: vec num cols (%d) != matrix num rows (%d)!\n", length, m.numRows);
		exit(0);
	}
	result.resize(m.numCols);
	for(int i = 0; i < m.numCols; i++)
	{
		result[i] = 0;
		for(int j = 0; j < m.numRows; j++)
			result[i] += m[j][i] * values[j];
	}
}

void vector::multiply(vector v, matrix& result)
{
	result.resize(length, v.length);
	for(int i = 0; i < length; i++)
		for(int j = 0; j < v.length; j++)
			result[i][j] = values[i] * v.values[j];
}

void vector::multiply(vector v, double& result)
{
	result = dotProduct(v);
}

void vector::print()
{
	printf("< ");
	for(int i = 0; i < length; i++)
		printf("%.8f  ", values[i]);
	printf(" >");
}

/********************************************************************************************************************/
/********************************************************************************************************************/

matrix::matrix(int rows, int cols)
{
	values = new vector[rows];
	for(int i = 0; i < rows; i++)
		values[i].resize(cols);
	numRows = rows;
	numCols = cols;
}

matrix::matrix(int rows, int cols, double fillVal)
{
	values = new vector[rows];
	for(int i = 0; i < rows; i++)
	{
		values[i].resize(cols);
		for(int j = 0; j < cols; j++)
			values[i][j] = fillVal;	
	}	
	numRows = rows;
	numCols = cols;
}

matrix::matrix(vector* vals, int rows, int cols)
{
	numRows = rows;
	numCols = cols;
	values = new vector[rows];
	for(int i = 0; i < rows; i++)
		values[i] = vals[i];
}

const matrix matrix::operator = (const matrix m)
{
	cout << "in matrix::=\n";
	if(numRows != m.numRows || numCols != m.numCols)
		resize(m.numRows, m.numCols);
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			values[i][j] = m.values[i][j];
	return m;
}

matrix matrix::operator + (matrix m)
{
	if(numRows != m.numRows || numCols != numCols)
	{
		cout << "Error in matrix::operator +: matrices have different sizes (" << numRows << " x " << numCols << " and " << m.numRows
			  << " x " << m.numCols << ")!\n";
		exit(0);
	}
	matrix temp(numRows, numCols);
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			temp[i][j] = values[i][j] + m.values[i][j];
	return temp;
}

void matrix::operator += (matrix m)
{
	if(numRows != m.numRows || numCols != m.numCols)
	{
		printf("Error in matrix::operator +=: matrices have sizes %d x %d and %d x %d (not equal)!\n", numRows, numCols, m.numRows, m.numCols);
		exit(0);
	}
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			values[i][j] += m.values[i][j];
}

matrix matrix::operator - (matrix m)
{
	if(numRows != m.numRows || numCols != numCols)
	{
		cout << "Error in matrix::operator -: matrices have different sizes (" << numRows << " x " << numCols << " and " << m.numRows
			  << " x " << m.numCols << ")!\n";
		exit(0);
	}
	matrix temp(numRows, numCols);
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			temp[i][j] = values[i][j] - m.values[i][j];
	return temp;
}

void matrix::operator -= (matrix m)
{
	if(numRows != m.numRows || numCols != m.numCols)
	{
		printf("Error in matrix::operator -=: matrices have sizes %d x %d and %d x %d (not equal)!\n", numRows, numCols, m.numRows, m.numCols);
		exit(0);
	}
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			values[i][j] -= m.values[i][j];
}

matrix matrix::operator * (double scalar)
{
	matrix m = (*this);
	m.scale(scalar);
	return m;
}

void matrix::operator *= (double scalar)
{
	scale(scalar);
}

matrix matrix::operator * (matrix m)
{
	if(numCols != m.numRows)
	{
		printf("Error in matrix:: operator *=: columns of A do not equal rows of B (%d and %d)!\n", numCols, m.numRows);
		exit(0);
	}
	matrix temp(numRows, m.numCols);
	multiply(m, temp);
	return temp;
}

void matrix::operator *= (matrix m)
{
	if(numCols != m.numRows)
	{
		printf("Error in matrix:: operator *=: columns of A do not equal rows of B (%d and %d)!\n", numCols, m.numRows);
		exit(0);
	}
	matrix temp(numRows, m.numCols);
	multiply(m, temp);
	(*this) = temp;
}

vector matrix::operator * (vector v)
{
	if(v.length != numCols)
	{
		cout << "Error in matrix::* by vector: matrix has size " << numRows << " x " << numCols << ", vector has size " << v.length << "!\n";
		exit(0);
	}
	vector temp(numRows);
	for(int i = 0; i < numRows; i++)
		temp.values[i] = v.dotProduct(values[i]);
	return temp;
}

const int matrix::getRows()
{
	return numRows;
}

const int matrix::getCols()
{
	return numCols;
}

vector& matrix::operator [] (int index)
{
	return values[index];
}

matrix matrix::submatrix(int row, int col, int rowSize, int colSize)
{
	if(row + rowSize > numRows || col + colSize > numCols)
	{
		cout << "Error in matrix::submatrix: matrix has size  " << numRows << " x " << numCols << " and submatrix from (" << row << ", " << col
			  << ") to (" << row + rowSize - 1 << ", " << col + colSize - 1 << ") is requested!\n";
		exit(0);
	}
	matrix temp(rowSize, colSize);
	for(int i = 0; i < rowSize; i++)
		for(int j = 0; j < colSize; j++)
			temp.values[i][j] = values[i + row][j + col];
	return temp;
}

matrix matrix::identity(int size)
{
	matrix temp(size, size, 0);
	for(int i = 0; i < size; i++)
		temp.values[i][i] = 1;
	return temp;
}

void matrix::multiply(matrix m, matrix& result) //matrix x matrix
{
	if(numCols != m.numRows)
	{
		printf("Error in matrix mult by matrix: 1's columns (%d) not equal to 2's rows (%d)!\n", numCols, m.numRows);
		exit(0);
	}
	result.resize(numRows, m.numCols);
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < m.numCols; j++)
		{
			result.values[i][j] = 0;
			for(int k = 0; k < numCols; k++)
				result.values[i][j] += values[i][k] * m.values[k][j];
		}
}

void matrix::multiply(vector v, vector& result) //matrix x vert vector
{
	if(numCols != v.length)	
	{		
		printf("Error in matrix mult by vector: matrix columns (%d) not equal to vector length (%d)!\n", numCols, v.length);
		exit(0);
	}
	result.resize(numRows);	
	for(int i = 0; i < numRows; i++)
		result.values[i] = v.dotProduct(values[i]);
}

void matrix::fill(double datum)
{
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			values[i][j] = datum;
}

void matrix::scale(double scalar)
{
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			values[i][j] *= scalar;
}

void matrix::transpose()
{
	matrix temp(numCols, numRows);
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numCols; j++)
			temp.values[j][i] = values[i][j];
	delete[] values;
	values = temp.values;
	int i = numRows;
	numRows = numCols;
	numCols = i;
}

void matrix::resize(int newRows, int newCols)
{
	if(!numRows && !numCols)
	{
		values = new vector[numRows = newRows];
		for(int i = 0; i < numRows; i++)
			values[i].resize(numCols = newCols);
		return;
	}
	int minRows = min(numRows, newRows), minCols = min(numCols, newCols);
	matrix temp(newRows, newCols);
	for(int i = 0; i < minRows; i++)
		for(int j = 0; j < minCols; j++)
			temp.values[i][j] = values[i][j];
	delete[] values;
	values = temp.values;
	numRows = newRows;
	numCols = newCols;
}

void matrix::swapRows(int r1, int r2)
{
	for(int i = 0; i < numCols; i++)
		swap(values[r1][i], values[r2][i]);
}

matrix matrix::augment(matrix m)
{
	if(m.numRows != numRows)
	{
		cout << "Error in matrix::augment: matrices have different row sizes (" << numRows << " and " << m.numRows << ")!\n";
		exit(0);
	}
	int cols = numCols;
	matrix temp(numRows, numCols + m.numCols);
	for(int i = 0; i < numRows; i++)
	{
		for(int j = 0; j < cols; j++)
		{
			temp.values[i][j] = values[i][j];
			temp.values[i][j + cols] = m.values[i][j];
		}
	}
	return temp;
}

matrix matrix::rref()
{
	matrix temp = (*this);
	int colNum = 0;
	for(int i = 0; i < numRows; i++)
	{
		colNum = temp.firstNonzeroColumn(colNum, i);
		if(colNum == -1) return temp;
		if(temp.values[i][colNum] == 0)
			temp.swapRows(i, temp.firstNonzeroColumnEntry(colNum));
		temp[i].scale(1 / temp.values[i][colNum]);
		for(int j = 0; j < numRows; j++)
			if(j != i)
				temp.values[j] -= temp.values[i] * temp.values[j][colNum];
		colNum++;
	}
	return temp;
}

double matrix::trace() //trace: sum of elements on diagonal of square matrix
{
	if(numRows != numCols)
	{
		printf("Error in matrix::trace(): matrix is not square (size is %d by %d)!\n", numRows, numCols);
		exit(0);
	}
	double value = 0;
	for(int i = 0; i < numRows; i++)
		value += values[i][i];	
	return value;
}

double matrix::det() //determinant of square matrix
{
	if(numRows != numCols)
	{
		printf("Error in matrix::det(): matrix is not square (size is %d by %d)!\n", numRows, numCols);
		exit(0);
	}
	if(numRows == 1) return values[0][0];	
	else if(numRows == 2) return values[0][0] * values[1][1] - values[1][0] * values[0][1];
	else if(numRows == 3) //top-row expansion
	{
		return values[0][0] * (values[1][1] * values[2][2] - values[2][1] * values[1][2]) 
				 - values[0][1] * (values[1][0] * values[2][2] - values[2][0] * values[1][2]) 
				 + values[0][2] * (values[1][0] * values[2][1] - values[2][0] * values[1][1]);
	}
	else
	{
		printf("Error in matrix::det(): determinant not implemented for 4x4 or larger matrix (size is %d by %d)!\n", numRows, numCols);
		exit(0);
	}
}

bool matrix::isSymmetric()
{
	if(numRows != numCols) return false;
	for(int i = 1; i < numRows; i++)
		for(int j = 0; j < i; j++)
			if(values[i][j] != values[j][i])
				return false;
	return true;
}

int matrix::firstNonzeroColumn(int startCol, int startRow) //return col num
{
	for(int i = startCol; i < numCols; i++)
		for(int j = startRow; j < numRows; j++)
			if(values[j][i] != 0)
				return i;
	return -1;
}

int matrix::firstNonzeroColumnEntry(int col) //return row num
{
	for(int i = 0; i < numRows; i++)
		if(values[i][col] != 0)
			return i;
	return -1;
}

void matrix::print()
{
	printf("\n");
	for(int i = 0; i < numRows; i++)
	{
		for(int j = 0; j < numCols; j++)
			printf("%.8f  ", values[i][j]);
		printf("\n");
	}
}

/* Replaces given matrix with its inverse.  Uses LU decomposition, then finds
inverse column-by-column.  Taken from "Numerical Recipies in C."  Currently
handles up to a 10x10 Matrix, although this can be easily changed.
Returns whether successful. */
bool matrix::invert()
{
	if(numRows != numCols)
	{
		printf("Error in matrix::invert(): matrix is not square (size is %d by %d)!\n", numRows, numCols);
		exit(0);
	}
	int size = numRows;
	(*this) = augment(identity(size));
	rref();
	(*this) = submatrix(0, size, size, size);
	return true;
	/*
	matrix LUmatrix;
	double* col;
	double d;
	int* index;
	if(numRows == 2) //special case of 2 x 2 matrix
	{
		double a = values[0][0], b = values[0][1], c = values[1][0], d = values[1][1], det = d * a - b * c;
		values[0][0] =  d / det;		
		values[0][1] = -b / det;
		values[1][0] = -c / det;		
		values[1][1] =  a / det;
	} 
	else 
	{
		//allocate space
		col = new double[numRows];
		index = new int[numRows * sizeof(int)];
		//Copy our matrix into the holding area
		LUmatrix = (*this);
		//Do inversion via LU decomposition
		if(LUmatrix.LUdecomp(index, &d)) //if error return code not zero
		{
			return false;
		}
		for(int j = 0; j < numRows; j++) 
		{
			for(int i = 0; i < numRows; i++)
				col[i] = 0;
			col[j] = 1;
			LUmatrix.LUbacksub(index, col);
			for(int i = 0; i < numRows; i++)
				values[j][i] = col[i];		
		}
	}
	return true;*/
}

//values of matrixNormType:
//MTX_NORM_ONE: max sum of abs values of elements in a column
//MTX_NORM_TWO: square root of sum of squares of all elements
//MTX_NORM_MAX: max abs val of an element
//MTX_NORM_INF: max sum of abs values of elements in a row
double matrix::norm(matrixNormType t)
{
	double norm, sum;
	switch(t) 
	{
	case MTX_NORM_ONE:
	 	sum = 0;
  		for(int i = 0; i < numRows; i++)
			sum += fabs(values[i][0]);
		norm = sum;
  		for(int j = 1; j < numCols; j++) 
		{
			sum = 0;
			for(int i = 0; i < numRows; i++)
			  sum += fabs(values[i][j]);
			if(norm < sum) norm = sum;
  		}
  		return norm;
	case MTX_NORM_TWO:
	 	sum = 0;
  		for(int i = 0; i < numRows; i++)
			for(int j = 0; j < numCols; j++)
	  			sum += values[i][j] * values[i][j];
  		return sqrt(sum);
	case MTX_NORM_MAX:
	 	norm = fabs(values[0][0]);
  		for(int i = 0; i < numRows; i++)
			for(int j = 0; j < numCols; j++)
				if(norm < fabs(values[i][j]))
					norm = fabs(values[i][j]);
  		return norm;
	case MTX_NORM_INF:
	 	sum = 0;
  		for(int j = 0; j < numCols; j++)
			sum += fabs(values[0][j]); 
		norm = sum;
  		for(int i = 1; i < numRows; i++) 
		{
			sum = 0;
			for(int j = 0; j < numCols; j++)
	  			sum += fabs(values[i][j]);
			if(norm < sum) norm = sum;
  		}
  		return norm;
	default:
  		printf("Error in matrix::norm(): invalid norm requested!\n");
  		exit(0);
	}
}

/*****************************************************************************
**  Name:LUdecomp
**  Descrip: Replaces m with its LU decomposition.
**Taken from "Numerical Recipies in C," p.43
**  Returns: 0 on success, -1 on error. 
*****************************************************************************/
int matrix::LUdecomp(int* index, double* d)
{
	int imax, i, j, k;
	double big, dum, sum, temp;
	vector vv(numRows);
	(*d) = 1;
	for(i = 0; i < numRows; i++) 
	{
		big = 0.0;
		for(j = 0; j < numRows; j++)
			if((temp = fabs(values[j][i])) > big) 
				big = temp;
		if(big == 0) 
		{
			printf("LUdecomp: singular Matrix!\n");
			return -1;
		}
		vv[i] = 1 / big;
	}
	for(j = 0; j < numRows; j++) 
	{
		for(i = 0; i < j; i++) 
		{
			sum = values[j][i];
			for(k = 0; k < i; k++)
				sum -= values[k][i] * values[j][k];
			values[j][i] = sum;		
		}
		big = 0;
		for(i = j; i < numRows; i++) 
		{
			sum = values[j][i];
			for(k = 0; k < j; k++)
				sum -= values[k][i] * values[j][k];
			values[j][i] = sum;			
			if((dum = vv[i] * fabs(sum)) >= big) 
			{
				big = dum;
				imax = i;
			}
		}
		if(j != imax) 
		{
			for(k = 0; k < numRows; k++) 
				swap(values[k][imax], values[k][j]);
			(*d) *= -1;
			vv[imax] = vv[j];
		}
		index[j] = imax;		
		if(values[j][j] == 0) 
		{
			printf("LUdecomp: hit singular pivot!\n");
			return -1;
		}
		if(j != numRows - 1) 		
		{			
			dum = 1 / values[j][j];
			for(i = j + 1; i < numRows; i++)
				values[j][i] *= dum;		
		}
	}
	return 0;
}

/*****************************************************************************
**  Name:LUbacksub
**  Descrip: Solves a set of linear equations mX = B where matrix m is the LU
**decomposition of the original matrix m.
**Taken from "Numerical Recipies in C," p.44
**  Returns: 0 on success, -1 on error.
*****************************************************************************/
int matrix::LUbacksub(int* index, double* b)
{
	int i, j, ii = -1, ip;	
	double sum;
	for(i = 0; i < numRows; i++) 
	{
		ip = index[i];
		sum = b[ip];
		b[ip] = b[i];
		if(ii > -1)
			for(j = ii; j <= i - 1; j++)
				sum -= values[j][i] * b[j];
		else if(sum) ii = i;		
		b[i] = sum;
	}
	for(i = numRows - 1; i >= 0; i--) 
	{
		sum = b[i];
		for(j = i + 1; j < numRows; j++) 
			sum -= values[j][i] * b[j];
		b[i] = sum / values[i][j];
	}
	return 0;
}

void matrix::svd(matrix& u, vector& w, matrix& v)
{
	cout << "in svd\n";
	//initialize output
	const int minDim = min(numRows, numCols);
	u.resize(numRows, numRows);
	w.resize(minDim);
	v.resize(numCols, numCols);
	//parameters to dsvdc:
	//input
	double* a = new double[numRows * numCols]; //will hold the data in *this
	integer nRows = numRows, nCols = numCols;
	double* workArray = new double[numRows];
	integer jobCodes = 11; //return right sing vecs in V, left in U
	//return
	double* singularValues = new double[min(numRows + 1, numCols)];
	double* returnValues = new double[numCols]; //not useful
	double* leftSingularVectors = new double[numRows * numRows]; //not used if jobCodes < 10
	double* rightSingularVectors = new double[numCols * numCols];
	integer returnErrorCode; //should be 0 if all OK
	//initialize a
	for(int i = 0; i < numCols; i++)
		for(int j = 0; j < numRows; j++)
			a[i * numRows + j] = values[j][i];
	//call Linpack routine
	/*cout << "parameters are: " << a << ", " << nRows << ", " << nCols << ", " << singularValues
		  << ", " << returnValues << ", " << leftSingularVectors << ", " << rightSingularVectors
		  << ", " << workArray << ", " << jobCodes << ", " << returnErrorCode << endl;*/
	dsvdc_(a, &nRows, &nRows, &nCols, singularValues, returnValues, leftSingularVectors, &nRows, 
			rightSingularVectors, &nCols, workArray, &jobCodes, &returnErrorCode);
	//check for errors
	if(returnErrorCode != 0)
	{
		printf("Error in matrix::svd(): SVD could not be computed!\n");
		printf("%d singular values were not found\n", returnErrorCode);
		exit(0);
	}
	//decode info
	for(int i = 0; i < numRows; i++)
		for(int j = 0; j < numRows; j++)
			u[i][j] = leftSingularVectors[j * numRows + i];
	for(int i = 0; i < minDim; i++)
		w[i] = singularValues[i];
	for(int i = 0; i < numCols; i++)
		for(int j = 0; j < numCols; j++)
			v[i][j] = rightSingularVectors[j * numCols + i];
	cout << "done in svd\n";
}