#include "Transform.h"
#include <assert.h>
#include <math.h>

CTransform::CTransform(){
    matrix = new double[16];
    LoadIdentity();
}

CTransform& CTransform::Transform(Vertex3d v){
    matrix[12] = v.x;
    matrix[13] = v.y;
    matrix[14] = v.z;
    return (*this);
}

CTransform& CTransform::Translate(Vertex3d v){
    matrix[12] += v.x;
    matrix[13] += v.y;
    matrix[14] += v.z;
    return (*this);
}

CTransform& CTransform::Scale(Vertex3d v){
    matrix[0] *= v.x;
    matrix[5] *= v.y;
    matrix[10] *= v.z;
    return (*this);
}

CTransform& CTransform::LoadIdentity(){

    matrix[0] = 1;
    matrix[1] = 0;
    matrix[2] = 0;
    matrix[3] = 0;
    matrix[4] = 0;
    matrix[5] = 1;
    matrix[6] = 0;
    matrix[7] = 0;
    matrix[8] = 0;
    matrix[9] = 0;
    matrix[10] = 1;
    matrix[11] = 0;
    matrix[12] = 0;
    matrix[13] = 0;
    matrix[14] = 0;
    matrix[15] = 1;
    return (*this);
}

int CTransform::I (int row, int col) const{
    assert( 0 <= row && row < 4 && 0 <= col && col < 4 );
    return 4*col + row;
}

void CTransform::Multiply(double* targetmatrix, double* multiplier){
    CTransform kProd;
    for (int iRow = 0; iRow < 4; iRow++){
        for (int iCol = 0; iCol < 4; iCol++){
            int i = I(iRow,iCol);
            kProd.matrix[i] = 0.0;
            for (int iMid = 0; iMid < 4; iMid++){
                kProd.matrix[i] +=
                    targetmatrix[I(iRow,iMid)]*multiplier[I(iMid,iCol)];
            }
        }
    }
    for(int i = 0; i < 16; i++){
        targetmatrix[i] = kProd.matrix[i];
    }
}

CTransform& CTransform::operator* (CTransform& rkM){
    Multiply(matrix,rkM.matrix);
    /*CTransform kProd;
    for (int iRow = 0; iRow < 4; iRow++){
        for (int iCol = 0; iCol < 4; iCol++){
            int i = I(iRow,iCol);
            kProd.matrix[i] = 0.0;
            for (int iMid = 0; iMid < 4; iMid++){
                kProd.matrix[i] +=
                    matrix[I(iRow,iMid)]*rkM.matrix[I(iMid,iCol)];
            }
        }
    }*/
    return (*this);
}

CTransform CTransform::operator*=(CTransform b){
    (*this) = ((*this) * b);
    return (*this);
}

double* CTransform::operator[] (int row){
    return &matrix[4*row];
}

Vertex3d CTransform::GetColumn(int column){
    Vertex3d v;
    v.x = matrix[column*4];
    v.y = matrix[column*4+1];
    v.z = matrix[column*4+2];
    return v;
}

Vertex3d CTransform::GetRow(int row){
    Vertex3d v;
    v.x = matrix[row];
    v.y = matrix[row+4];
    v.z = matrix[row+8];
    return v;
}

CTransform& CTransform::SetColumn(int column, Vertex3d v){
    matrix[column*4] = v.x;
    matrix[column*4+1] = v.y;
    matrix[column*4+2] = v.z;
    return (*this);
}

CTransform& CTransform::SetRow(int row, Vertex3d v){
    matrix[row] = v.x;
    matrix[row+4] = v.y;
    matrix[row+8] = v.z;
    return (*this);
}

CTransform& CTransform::Rotate(double angle, Vertex3d v){
    //angle %= 360*(PI/180);
    CTransform rot;

    rot[0][0] = 1 + (1-cos(angle))*(v.x*v.x-1);
    rot[1][0] = v.z*sin(angle)+(1-cos(angle))*v.x*v.y;
    rot[2][0] = -v.y*sin(angle)+(1-cos(angle))*v.x*v.z;

    rot[0][1] = -v.z*sin(angle)+(1-cos(angle))*v.x*v.y;
    rot[1][1] = 1 + (1-cos(angle))*(v.y*v.y-1);
    rot[2][1] = v.x*sin(angle)+(1-cos(angle))*v.y*v.z;

    rot[0][2] = v.y*sin(angle)+(1-cos(angle))*v.x*v.z;
    rot[1][2] = -v.x*sin(angle)+(1-cos(angle))*v.y*v.z;
    rot[2][2] = 1 + (1-cos(angle))*(v.z*v.z-1);

    Multiply(matrix,rot.matrix);

    return (*this);
}