#include <math.h>

#include "robot_bodypart.h"
#include "robot_vector.h"
#include "robot_matrix.h"
#include "robot_conversions.h"

Cbodypart::Cbodypart(int shape_type, Cvector color, Cvector scale, Cvector translate, Cvector rotate, Cvector pivot)
{
    this->shape_type = shape_type;

    this->color_r = color.x;
    this->color_g = color.y;
    this->color_b = color.z;

	this->scale_x = scale.x;
	this->scale_y = scale.y;
	this->scale_z = scale.z;

	this->translate_x = translate.x;
	this->translate_y = translate.y;
	this->translate_z = translate.z;

	this->rotate_x = rotate.x;
	this->rotate_y = rotate.y;
	this->rotate_z = rotate.z;

    this->pivot_x = pivot.x;
    this->pivot_y = pivot.y;
    this->pivot_z = pivot.z;

    InitTransformationMatrix();
}

Cbodypart::~Cbodypart()
{
}


void Cbodypart::InitScaleMatrix()
{
    scale_matrix.Identity();
    scale_matrix.element[0][0] = scale_x;
    scale_matrix.element[1][1] = scale_y;
    scale_matrix.element[2][2] = scale_z;
}

void Cbodypart::InitTranslationMatrix()
{
    translation_matrix.Identity();
    translation_matrix.element[0][3] = translate_x;
    translation_matrix.element[1][3] = translate_y;
    translation_matrix.element[2][3] = translate_z;
}

void Cbodypart::InitTranslationPivotMatrix()
{
    translation_pivot_matrix.Identity();
    translation_pivot_matrix.element[0][3] = pivot_x;
    translation_pivot_matrix.element[1][3] = pivot_y;
    translation_pivot_matrix.element[2][3] = pivot_z;
}

void Cbodypart::InitRotationMatrix()
{
    double cx = cos(d2r(rotate_x));
    double sx = sin(d2r(rotate_x));

    Cmatrix RotateX_matrix( 1,   0,   0,   0, 
                            0,   cx,  -sx, 0, 
                            0,   sx,  cx,  0, 
                            0,   0,   0,   1);


    double cy = cos(d2r(rotate_y));
    double sy = sin(d2r(rotate_y));

    Cmatrix RotateY_matrix( cy,  0,   sy,  0, 
                            0,   1,   0,   0, 
                            -sy, 0,   cy,  0, 
                            0,   0,   0,   1);


    double cz = cos(d2r(rotate_z));
    double sz = sin(d2r(rotate_z));

    Cmatrix RotateZ_matrix( cz,  -sz, 0,   0, 
                            sz,  cz,  0,   0, 
                            0,   0,   1,   0, 
                            0,   0,   0,   1);

    rotation_matrix = RotateX_matrix * RotateY_matrix * RotateZ_matrix;
}

void Cbodypart::InitTransformationMatrix()
{
    InitRotationMatrix();
    InitTranslationMatrix();
    InitTranslationPivotMatrix();
    InitScaleMatrix();

    transformation_matrix = translation_matrix * rotation_matrix * translation_pivot_matrix;
//    transformation_matrix.PrintMatrix();
}
