#include <unistd.h>
#include <stdio.h>
#include <linux/ps2/ee.h>
#include "packet.h"

void gifpacket_init(gifpacket_t *gpkt, u_long128 *buffer)
{
	gpkt->base=align128((u_int*)buffer);
	gpkt->current=(u_int*)gpkt->base;
	gpkt->dmatag=NULL;
}

void gifpacket_reset(gifpacket_t *gpkt)
{
	gpkt->current=(u_int*)gpkt->base;
	gpkt->dmatag=NULL;
}

u_long128 *gifpacket_terminate(gifpacket_t *gpkt)
{
	u_long128* const dma_current = align128(gpkt->current);
	if(gpkt->dmatag)
	{
		u_int const count = (dma_current - gpkt->dmatag) - 1;
		((u_int*)gpkt->dmatag)[0] += count;
	}
	gpkt->current = (u_int*)dma_current;
	gpkt->dmatag = NULL;
	return dma_current;
}

int  gifpacket_size(gifpacket_t *gpkt)
{
	return ((u_int)gpkt->current - (u_int)gpkt->base) / sizeof(u_long128);
}


void gifpacket_addgsdata(gifpacket_t *gpkt, u_long64 data)
{
	*((u_long64*)gpkt->current) = data;
	gpkt->current+=sizeof(u_long64)/sizeof(u_int);
}


void gifpacket_addgspacked(gifpacket_t* gpkt, u_long128 data)
{
	*((u_long128*)gpkt->current) = data;
	gpkt->current += sizeof(u_long128) / sizeof(u_int);
}

void gifpacket_addgsvec3(gifpacket_t* gpkt, Vec3* vec)
{
	qword_t data;
	data.f32[0] = vec->x;
	data.f32[1] = vec->y;
	data.f32[2] = vec->z;
	data.f32[3] = 1.0f;

	gifpacket_addgspacked(gpkt, data.ul128);
}

void gifpacket_addgsvec4(gifpacket_t* gpkt, Vec4* vec)
{
	qword_t data;
	data.f32[0] = vec->x;
	data.f32[1] = vec->y;
	data.f32[2] = vec->z;
	data.f32[3] = vec->w;

	gifpacket_addgspacked(gpkt, data.ul128);
}

void gifpacket_addgsquat(gifpacket_t* gpkt, Quat* quat)
{
	qword_t data;
	data.f32[0] = quat->V.x;
	data.f32[1] = quat->V.y;
	data.f32[2] = quat->V.z;
	data.f32[3] = quat->s;

	gifpacket_addgspacked(gpkt, data.ul128);
}

void gifpacket_addgsmat33(gifpacket_t* gpkt, Mat33* mat)
{
	Vec3 temp = GetRow(*mat, 0);
	gifpacket_addgsvec3(gpkt, &temp);
	temp = GetRow(*mat, 1);
	gifpacket_addgsvec3(gpkt, &temp);
	temp = GetRow(*mat, 2);
	gifpacket_addgsvec3(gpkt, &temp);
}

void gifpacket_addgsmat44(gifpacket_t* gpkt, Mat44* mat)
{
	Vec4 temp = GetRow(*mat, 0);
	gifpacket_addgsvec4(gpkt, &temp);
	temp = GetRow(*mat, 1);
	gifpacket_addgsvec4(gpkt, &temp);
	temp = GetRow(*mat, 2);
	gifpacket_addgsvec4(gpkt, &temp);
	temp = GetRow(*mat, 3);
	gifpacket_addgsvec4(gpkt, &temp);
}

void gifpacket_dump(gifpacket_t *gpkt)
{
	int size=gifpacket_size(gpkt);
	int i;
	unsigned int *dat=(unsigned int *)gpkt->base;

	for (i=0; i<size; i++)
	{
		printf("-gifpacket: 0x%.8x 0x%.8x 0x%.8x 0x%.8x\n", dat[0], dat[1], dat[2], dat[3]);
		dat+=4;
	}
}