//
// ==============================================================
//
//                      ECS 110 - Assignment #1
//
//                      Justin Legakis
//
//                      April 27, 1993
//
// ==============================================================
//

//
// ======================================================
//
//       Implementation of Class
//
//          JLmatrix
//
// ======================================================
//

#include <stdlib.h>
#include <math.h>
#include <iostream.h>
#include <string.h>
#include "matrix.h"

//
// ======================================================
//
//       Constructors and destructor
//
// ======================================================
//

// Creates matrix of zero's
JLmatrix::JLmatrix() {

    _size = 4;
    _data = new real[16];
    for (int i=0; i<16; i++)
    	_data[i] = 0.0;
}

JLmatrix::JLmatrix(const int s) {

    _size = s;
    _data = new real[s*s];
    for (int i=0; i<(s*s); i++)
	    _data[i] = 0.0;
}

// Creates matrix, and initializes to values of an array
JLmatrix::JLmatrix(const int s, const real *a) {

    _size = s;
    _data = new real[s*s];
    for (int i=0; i<(s*s); i++)
	    _data[i] = a[i];
}

// Copy constructor
JLmatrix::JLmatrix(const JLmatrix& m) {

    _size = m._size;
    _data = new real[_size*_size];
    for (int i=0; i<(_size*_size); i++)
		_data[i] = m._data[i];
}

// Construct identity matrix:
JLmatrix::JLmatrix(const char) {

// if (char == 'I') ...

    _size = 4;
    _data = new real[_size*_size];

    for (int y=0; y<_size; y++)
        for (int x=0; x<_size; x++)
            _data[y*_size+x] = (x==y);
}

// Destructor
JLmatrix::~JLmatrix() {

    delete [] _data;
}

//
// Creates and returns an identity matrix
//

JLmatrix IJLmatrix(const int s) {

    JLmatrix m(s);

    for (int y=0; y<s; y++)
        for (int x=0; x<s; x++)
            m._data[y*s+x] = ((x==y) ? 1 : 0);

    return m;
}

void JLmatrix::SetSize(int s) {

	if (s != _size) {
		delete [] _data;
		_size = s;
		_data = new real[_size*_size];
	}
}

void JLmatrix::CopyFromGLMatrix(const Matrix m) {

    SetSize(4);
    for (int i=0; i<16; i++)
		_data[i] = m[i/4][i%4];
}

//
// ======================================================
//
//        Operator =
//
// ======================================================
//

JLmatrix& JLmatrix::operator=(const JLmatrix& m) {

    if (this == &m) return (*this);

    for (int y=0; y<m._size; y++)
        for (int x=0; x<m._size; x++)
            _data[y*m._size+x] = m._data[y*m._size+x];

    return (*this);
}

//
// ======================================================
//
//          Comparison operators ==, !=
//
// ======================================================
//

int operator==(const JLmatrix& m1, const JLmatrix& m2) {

    if (m1._size != m2._size) return 0;

    for (int y=0; y<m1._size; y++)
        for (int x=0; x<m1._size; x++)
            if (m1._data[y*m1._size+x] != m2._data[y*m1._size+x]) return 0;

    return 1;
}

int operator!=(const JLmatrix& m1, const JLmatrix& m2) {

    return !(m1==m2);
}

//
// ======================================================
//
//             JLmatrix output routines
//
//    printEntry() and printJLmatrixRow() are called
//    only by functions in matrix.C.  They are not
//    available to the user.
//
// ======================================================
//

ostream& printEntry(ostream& co, real d) {

// *** Prints a real so that it takes up EXACTLY 10 characters  ***
// *** (Not an easy task, if you have to cover all possible cases ***

//      This is very messy, because in order to make the
//   matrix look just right on the screen, different
//   formatting commands were required for different ranges
//   of numbers.  It looks ridiculous, but it's a small
//   price to pay for perfection.

/* 
    if (d >= 0)
    	co << " ";
    else {
        d = -d;
        co << "-";
    }

    if (d>=1000)
        return (co.form("%10.4e",d));
    if (d>=100)
        if (d == ((int) d / 100 * 100))
            return (co.form("%8.6f",d));
        else if (d == ((int) d / 10 * 10))
            return (co.form("%9.6f",d));
        else
            return (co.form("%10.6f",d));
    if (d>=10)
        if (d == ((int) d / 10 * 10))
            return (co.form("%9.7f",d));
        else
            return (co.form("%10.7f",d));
    if ((d>=0.01) || (d==0.0))
        return (co.form("%10.8f",d));
    return (co.form("%10.4e",d));
*/
    co << d << " ";
    return co;
}

// *** Prints one row of matrix ***
ostream& printJLmatrixRow(ostream& co, const JLmatrix& m, const int row) {

    if (row==0) co << "|~ ";
    else if (row==(m.size()-1)) co << "|_ ";
    else co << "|  ";

    for (int x=0; x<m.size(); x++) {
    	real e = m.value(x,row);
        printEntry(co, e);
        co << " ";
    }

    if (row==0) co << "~|";
    else if (row==(m.size()-1)) co << "_|";
    else co << " |";

    return co;
}

//
// ======================================================
//
//         operator <<
//
// ======================================================
//

ostream& operator<<(ostream& co, const JLmatrix& m) {
 
    for (int y=0; y<m._size; y++) {
        printJLmatrixRow(co, m, y);
        co << endl;
    }

    return co;
}

//
// ======================================================
//
//             printJLmatrixWithLabel()
//
//    Specialized function to print a matrix, preceded
//    by a character string, and optionally followed by
//    a second string.  The suffix string may be omitted
//
// ======================================================
//

void printJLmatrixWithLabel(const char* prefix, const JLmatrix& m,
                          const char* suffix) {

    int space;

    for (int y=0; y<m._size; y++) {
        if (y==(m._size/2))
            cout << prefix;
        else
            for (space = 0; space < int(strlen(prefix)); space++)
                cout << " ";

        printJLmatrixRow(cout, m, y);

        if (y==(m._size/2))
            cout << suffix;
        cout << endl;
    }
    cout << endl;
}

//
// ======================================================
//
//       arithmetic operations
//
// ======================================================
//

// *** JLmatrix addition: ***

JLmatrix operator+(const JLmatrix& m1, const JLmatrix& m2) {

    if (m1._size != m2._size) {
	cout << "Error!  Cannot add different size matrices." << endl;
	exit(0);
    }

    JLmatrix mm(m1._size);

    for (int i=0; i<(m1._size*m1._size); i++)
        mm._data[i] = m1._data[i] + m2._data[i];

    return mm;
}

// *** JLmatrix negation: ***

JLmatrix operator-(const JLmatrix& m) {

    JLmatrix mm(m._size);

    for (int i=0; i<(m._size*m._size); i++)
        mm._data[i] = -m._data[i];

    return mm;
}

// *** JLmatrix subtraction: ***

JLmatrix operator-(const JLmatrix& m1, const JLmatrix& m2) {

    if (m1._size != m2._size) {
	cout << "Error!  Cannot subtract different size matrices." << endl;
	exit(0);
    }

    JLmatrix mm(m1._size);

    for (int i=0; i<(m1._size*m1._size); i++)
        mm._data[i] = m1._data[i] - m2._data[i];

    return mm;
}

// *** Multiplication of matrix by scalar: ***

JLmatrix operator*(const JLmatrix& m, const real d) {

    JLmatrix mm(m._size);

    for (int i=0; i<(m._size*m._size); i++)
        mm._data[i] = m._data[i] * d;

    return mm;
}

// *** Multiplication of scalar by matrix: ***

JLmatrix operator*(const real d, const JLmatrix& m) {

    return m * d;
}

// *** JLmatrix multiplication: ***

JLmatrix operator*(const JLmatrix& m1, const JLmatrix& m2) {

    if (m1._size != m2._size) {
	cout << "Error!  Cannot multiply different size matrices." << endl;
	exit(0);
    }

    JLmatrix mm(m1._size);

    for (int y=0; y<m1._size; y++)
        for (int x=0; x<m1._size; x++)
            for (int i=0; i<m1._size; i++)
                mm._data[y*m1._size+x] += m1._data[y*m1._size+i] *
					  m2._data[i*m1._size+x];

    return mm;
}

//
// ======================================================
//
//      +=, -=, and *= operations
//
// ======================================================
//


JLmatrix& JLmatrix::operator+=(const JLmatrix& m) {

    *this = *this + m;
    return (*this);
}

JLmatrix& JLmatrix::operator-=(const JLmatrix& m) {

    *this = *this + (-m);
    return (*this);
}

JLmatrix& JLmatrix::operator*=(const real d) {

    *this = (*this) * d;
    return (*this);
}

JLmatrix& JLmatrix::operator*=(const JLmatrix& m) {

    *this = *this * m;
    return (*this);
}

//
// ======================================================
//
//        Calculates transpose of matrix
//
// ======================================================
//

JLmatrix JLmatrix::transpose() {

    JLmatrix m(_size);

    for (int y=0; y<_size; y++)
        for (int x=0; x<_size; x++)
            m._data[y*_size+x] = _data[x*_size+y];

    return m;
}

//
// ======================================================
//
//       Calculates determinant of matrix
//
// ======================================================
//

real JLmatrix::determinant() {

//
//  If desired, this function can be expanded
//  to support matrices of other sizes
//

    real d;

    if (_size != 3) {
        cerr << "Error: determinant() only implemented for 3x3 matrices";
	cerr << endl;
        return 0;
    }

    d = _data[0] * (_data[4]*_data[8] - _data[5]*_data[6]);
    d -= _data[1] * (_data[3]*_data[8] - _data[5]*_data[6]);
    d += _data[2] * (_data[3]*_data[7] - _data[4]*_data[6]);

    return d;
}

void JLmatrix::WriteValues(ostream& co) const {

    for (int y=0; y<_size; y++) {
	for (int x=0; x<_size; x++)
	    co << value(x,y) << " ";
	co << endl;
    }
}

void JLmatrix::ReadValues(istream& ci) {

    real d;

    for (int y=0; y<_size; y++)
	for (int x=0; x<_size; x++) {
	    ci >> d;
	    set(x,y,d);
	}
}

//
// ======================================================
//
//       Preforms a row operation on a matrix
//
// ======================================================
//

JLmatrix& JLmatrix::RowOp(real Factor, int SrcRow, int DstRow) {

	for (int i=0; i<_size; i++)
		_data[DstRow*_size+i] += _data[SrcRow*_size+i]*Factor;

	return (*this);
}

//
// ======================================================
//
//       Calculates LU Decomposition of matrix
//
// ======================================================
//

int JLmatrix::LU(JLmatrix& L, JLmatrix& U) {

    JLmatrix E;

    U = *this;
    L = IJLmatrix(_size);
    for (int c=0; c<(_size-1); c++)
        for (int r=1; r<_size; r++)
            if ((c != r) && (U._data[r*U._size+c])) {
                E = IJLmatrix(_size);
                E.RowOp(-U._data[r*_size+c]/U._data[c*_size+c],c,r);
                U.RowOp(-U._data[r*_size+c]/U._data[c*_size+c],c,r);
                L *= (IJLmatrix(_size)*2.0f - E);
            }
    return 1;
}

void JLmatrix::Transpose(JLmatrix &T) {

	for (int i=0; i<_size; i++)
		for (int j=0; j<_size; j++)
			T._data[i+j*_size] = _data[j+i*_size];
}

void JLmatrix::Inverse(JLmatrix &I) {

	if (!_data) {
		cerr << "Error in Inverse(): matrix not square" << endl;
		exit(-1);
	}

	JLmatrix M(*this);
	I.SetSize(_size);
	I.SetToIdentity();

	real v;

	for (int c=0; c<_size; c++) {
		v = M.value(c, c);
		if (!v) {

			int i=c+1;
			int done;
			do {
				while ((i<_size) && (M.value(c, i) == 0))
					i++;
				done = 1;
				if (i < _size)
					for (int z=0; z<c; z++)
						if (M.value(z, i) != 0)
							done = 0;
				if (!done)
					i++;
			} while (!done);

			if (i >= _size) {
				cerr << "Error: Matrix not invertable" << endl;
				cerr << "c=" << c << endl;
				cerr << "v=" << v << endl;
				cerr << *this;
				cerr << M;
				exit(-1);
			}

			I.RowOp(1, i, c);
			M.RowOp(1, i, c);
			v = M.value(c, c);
		}
		if (v != 1.0f) {
			for (int c2=0; c2<_size; c2++) {
				M._data[c*_size+c2] /= v;
				I._data[c*_size+c2] /= v;
			}
			M.set(c, c, 1);
		}
		for (int r=0; r<_size; r++)
			if (r != c)
				if (M.value(c, r) != 0.0f) {
					I.RowOp(-(M.value(c, r)), c, r);
					M.RowOp(-(M.value(c, r)), c, r);
					M.set(c, r, 0);
				}
	}
}

real JLmatrix::MultRowByVector(int r, real *vec) {

	real result = 0;

	for (int i=0; i<_size; i++)
		result += vec[i] * _data[r*_size + i];

	return result;
}

JLmatrix& JLmatrix::Scale(real x, real y, real z) {

	if (_size != 4) {
		cerr << "Error in JLmatrix::Scale(): size != 4" << endl;
		exit(-1);
	}

	for (int j=0; j<=12; j+=4) {
		_data[j] *= x;
		_data[j+1] *= y;
		_data[j+2] *= z;
	}

	return *this;
}

JLmatrix& JLmatrix::XRotate(real theta) {

	if (_size != 4) {
		cerr << "Error in JLmatrix::Scale(): size != 4" << endl;
		exit(-1);
	}

	static JLmatrix rx;

	rx.set(1,1,cosf(theta)).set(2,1,sinf(theta));
	rx.set(1,2,-sinf(theta)).set(2,2,cosf(theta));
	rx.set(0,0,1).set(3,3,1);

	*this *= rx;

	return *this;
}

JLmatrix& JLmatrix::YRotate(real theta) {

	if (_size != 4) {
		cerr << "Error in JLmatrix::Scale(): size != 4" << endl;
		exit(-1);
	}

	static JLmatrix ry;

	ry.set(0,0,cosf(theta)).set(2,0,-sinf(theta));
	ry.set(0,2,sinf(theta)).set(2,2,cosf(theta));
	ry.set(1,1,1).set(3,3,1);

	*this *= ry;

	return *this;
}

JLmatrix& JLmatrix::ZRotate(real theta) {

	if (_size != 4) {
		cerr << "Error in JLmatrix::Scale(): size != 4" << endl;
		exit(-1);
	}

	static JLmatrix rz;

	rz.set(0,0,cosf(theta)).set(1,0,sinf(theta));
	rz.set(0,1,-sinf(theta)).set(1,1,cosf(theta));
	rz.set(2,2,1).set(3,3,1);

	*this *= rz;

	return *this;
}

JLmatrix& JLmatrix::Translate(real x, real y, real z) {

	if (_size != 4) {
		cerr << "Error in JLmatrix::Translate(): size != 4" << endl;
		exit(-1);
	}

	for (int j=0; j<=12; j+=4) {
		_data[j]   = _data[j]   + _data[j+3]*x;
		_data[j+1] = _data[j+1] + _data[j+3]*y;
		_data[j+2] = _data[j+2] + _data[j+3]*z;
	}

	return *this;
}

void JLmatrix::GetGLMatrix(Matrix &m) {

	if (_size != 4) {
		cerr << "Error: JLmatrix::GetGLMatrix() called, size != 4" << endl;
		exit(-1);
	}

    for (int i=0; i<16; i++)
		m[i/4][i%4] = _data[i];
}