#include "helperFunctions.h"
#include "../vectorMath/vmath.h"
#include "drawstuff.h"

void dsDrawPolygon(const Polygon &p, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	unsigned i;
	Vector3r v;
	float start[3], end[3];
	for (i = 0; i < p.size(); i++) {
		v = p.at(i);
		start[0] = v[0]; start[1] = v[1]; start[2] = v[2] + 0.1;
		if (i + 1 < p.size()) {
			v = p.at(i + 1);
			end[0] = v[0]; end[1] = v[1]; end[2] = v[2] + 0.1;
			dsDrawLine(start, end);
		}
	}
	dsSetColor(1, 1, 1);
}

void dsDrawVector(const dReal* position, const dReal* direction, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	const float startP[3] = {position[0], position[1], position[2]};
	const float endP[3] = {position[0]+direction[0],position[1]+direction[1],position[2]+direction[2]};	
	dsDrawLine(startP, endP);
	dsSetColor(1, 1, 1);
}

void dsDrawVector(const Vector3r &position, const Vector3r &direction, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	const float startP[3] = {position[0], position[1], position[2]};
	const float endP[3] = {position[0]+direction[0],position[1]+direction[1],position[2]+direction[2]};	
	dsDrawLine(startP, endP);
	dsSetColor(1, 1, 1);
}

void dsDrawVector(const PVector3r &pv, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	const float startP[3] = {pv._pos[0], pv._pos[1], pv._pos[2]};
	const float endP[3] = {pv._pos[0]+pv._dir[0],pv._pos[1]+pv._dir[1],pv._pos[2]+pv._dir[2]};	
	dsDrawLine(startP, endP);
	dsSetColor(1, 1, 1);	
}

void dsDrawLine(const Line &l, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	const float startP[3] = {l._a.x,l._a.y,l._a.z};	
	const float endP[3] = {l._b.x,l._b.y,l._b.z};	
	dsDrawLine(startP, endP);
	dsSetColor(1, 1, 1);
}

void dsDrawPoint(const dReal* position, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	float sides[3] = {0.1, 0.1, 0.1}; 
	dMatrix3 R1;
	dRFromAxisAndAngle(R1, 0, 0, 1, 0);
	const float pos[3] = {position[0], position[1], position[2]};
	dsDrawBox(pos, R1, sides);
	dsSetColor(1, 1, 1);
}

void dsDrawPoint(const Vector3r &position, const float* aColor)
{
	if (aColor != NULL) dsSetColor(aColor[0], aColor[1], aColor[2]);
	else dsSetColor(1,1,1);
	float sides[3] = {0.1, 0.1, 0.1}; 
	dMatrix3 R1;
	dRFromAxisAndAngle (R1, 0, 0, 1, 0);
	const float pos[3] = {position.x,position.y,position.z};
	dsDrawBox(pos, R1, sides);
	dsSetColor(1, 1, 1);
}
