#include "bones/BoneAnimationState.h"

#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "bones/Skeleton.h"

BoneAnimationState * BoneAnimationState_create(Skeleton * skeleton) {
	BoneAnimationState * self;
	
	self = malloc(sizeof(BoneAnimationState));
	BoneAnimationState_init(self, skeleton);
	return self;
}

void BoneAnimationState_init(BoneAnimationState * self, Skeleton * skeleton) {
	unsigned int boneIndex;
	
	self->skeleton = skeleton;
	self->boneOrientations = malloc(sizeof(Quaternion) * self->skeleton->numberOfBones);
	self->boneOffsets = malloc(sizeof(Vector3) * self->skeleton->numberOfBones);
	self->boneMatrices = malloc(sizeof(Matrix) * self->skeleton->numberOfBones);
	
	for (boneIndex = 0; boneIndex < self->skeleton->numberOfBones; boneIndex++) {
		self->boneOrientations[boneIndex] = Quaternion_identity();
		self->boneOffsets[boneIndex] = Vector3_zero();
		self->boneMatrices[boneIndex] = Matrix_identity();
	}
	
	self->dispose = BoneAnimationState_dispose;
	self->createCopy = BoneAnimationState_createCopy;
	self->updateBoneMatrices = BoneAnimationState_updateBoneMatrices;
}

void BoneAnimationState_initCopy(BoneAnimationState * self, BoneAnimationState * original) {
	self->skeleton = original->skeleton;
	self->boneOrientations = malloc(sizeof(Quaternion) * self->skeleton->numberOfBones);
	self->boneOffsets = malloc(sizeof(Vector3) * self->skeleton->numberOfBones);
	self->boneMatrices = malloc(sizeof(Matrix) * self->skeleton->numberOfBones);
	memcpy(self->boneOrientations, original->boneOrientations, sizeof(Quaternion) * self->skeleton->numberOfBones);
	memcpy(self->boneOffsets, original->boneOffsets, sizeof(Vector3) * self->skeleton->numberOfBones);
	memcpy(self->boneMatrices, original->boneMatrices, sizeof(Matrix) * self->skeleton->numberOfBones);
}

void BoneAnimationState_dispose(void * selfPtr) {
	BoneAnimationState * self = selfPtr;
	
	free(self->boneOrientations);
	free(self->boneOffsets);
	free(self->boneMatrices);
	free(self);
}

void * BoneAnimationState_createCopy(void * selfPtr) {
	BoneAnimationState * self = selfPtr;
	BoneAnimationState * copy;
	
	copy = BoneAnimationState_create(self->skeleton);
	BoneAnimationState_initCopy(copy, self);
	return copy;
}

void BoneAnimationState_updateBoneMatrices(void * selfPtr) {
	BoneAnimationState * self = selfPtr;
	unsigned int boneIndex;
	Matrix boneMatrix;
	
	for (boneIndex = 0; boneIndex < self->skeleton->numberOfBones; boneIndex++) {
		Vector3 position;
		
		if (self->skeleton->bones[boneIndex].parentIndex == -1) {
			boneMatrix = Matrix_identity();
		} else {
			boneMatrix = self->boneMatrices[self->skeleton->bones[boneIndex].parentIndex];
		}
		
		position = self->skeleton->bones[boneIndex].position;
		Matrix_translate(&boneMatrix, self->boneOffsets[boneIndex].x, self->boneOffsets[boneIndex].y, self->boneOffsets[boneIndex].z);
		Matrix_translate(&boneMatrix, position.x, position.y, position.z);
		Matrix_multiply(&boneMatrix, Quaternion_toMatrix(self->boneOrientations[boneIndex]));
		Matrix_translate(&boneMatrix, -position.x, -position.y, -position.z);
		
		self->boneMatrices[boneIndex] = boneMatrix;
	}
}