#include <stdio.h>
#include <math.h>

#include <GL/gl.h>
#include <GL/glu.h>

#include "poly.h"

Poly::Poly(Array* _points) {
	Poly(_points, Color(1,1,1));
}

Poly::Poly(Array* _points, Color _color) {
	size = _points->length();
	position = new Vec(0,0,0);
	rotation = new Vec(0,0,0);
	color = _color.clone();

#ifdef DEBUG
	printf("new Poly(%d)\n", size);
#endif
	initpoints = new Array(_points);

	setCenter(calcCenter());
}

Poly::~Poly() {
	delete position;
	delete rotation;
	delete color;
	delete initpoints;
}

/*
* set the position where the (rotation-)center of
* the polygon is located
*/
void Poly::setCenter(Vec c) {
	delete position;
	position = c.clone();
}

/*
* initial rotation relative to rotation center
*/
Vec Poly::getInitRotation(Vec& v) {
	return Vec(atan2(v.y(), v.z()), atan2(v.x(), v.z()), atan2(v.y(), v.x()));
}

/*
* calculate the center relative to all vertices.
*/
Vec Poly::calcCenter() {
	Vec s (0,0,0);
	Vec* t;
	initpoints->reset();
	while ( t = (Vec*)initpoints->next() ) {
		s = s+*t;
	}
	return s / ((float)size);
}

/*
* reset the position of all vertices (ie. translate your
* polygon to an absolute position)
*/
void Poly::setPosition(Vec newcenter) {
	Vec diff = newcenter - (*position);
	delete position;
	position = newcenter.clone();
	Vec* t;
	initpoints->reset();
	while ( t = (Vec*)initpoints->next() ) {
		t->add(diff);
	}
}

//additional rotation in arcs
void Poly::rotate(Vec rot) {
	rotation->add(rot);
}

Vec Poly::rotate_x(Vec& c) {
	Vec initrot = getInitRotation(c);
	Vec rot = initrot + *rotation;
	float r = Vec(0, c.y(), c.z()).length();
	return Vec (c.x(), r*cos(rot.x()), r*sin(rot.x()));
}

Vec Poly::rotate_y(Vec& c) {
	Vec initrot = getInitRotation(c);
	Vec rot = initrot + *rotation;
	float r = Vec(c.x(), 0, c.z()).length();
	return Vec (r*sin(rot.y()), c.y(), r*cos(rot.y()));
}

Vec Poly::rotate_z(Vec& c) {
	Vec initrot = getInitRotation(c);
	Vec rot = initrot + *rotation;
	float r = Vec(c.x(), c.y(), 0).length();
	return Vec (r*cos(rot.z()), r*sin(rot.z()), c.z());
}

void Poly::draw() {
	glBegin (GL_POLYGON);

	//Vec v1 = *((Vec*)initpoints->get(2)) - *((Vec*)initpoints->get(0));
	//Vec v2 = *((Vec*)initpoints->get(1)) - *((Vec*)initpoints->get(0));
	//glNormal3fv(  v1.cross(v2)+ *(Vec*)((*initpoints)[0]).c  );
	//glNormal3fv(  v1.cross(v2) + ((Vec*)initpoints->get(0))->c  );

	Vec* t;
	Vec* mv;
	initpoints->reset();
	while ( (t = (Vec*)initpoints->next()) ) {
		color->drawColor();
		Vec out = (*t - *position);
		out = rotate_x(out);
		out = rotate_y(out);
		out = rotate_z(out);
		mv = (*position + out).clone();
		glVertex3fv(mv->c);
	}
	glEnd();
}