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

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

#include "poly.h"
#include <vector>
#include <string>

using namespace std;

Poly::Poly() {
	delta = 0.0f;
	position = new Vec(0,0,0);
	rotation = new Vec(0,0,0);
	color = new Color(1,0,0);
	normal = new Vec(0,0,0);
	setCenter(Vec(0,0,0));
}

Poly::Poly(std::vector<Vec*> & _points) {
	Poly(_points, Color(1,0,0));
}

Poly::Poly(std::vector<Vec*> & _points, Color _color) {
	delta = 0.0f;
	position = new Vec(0,0,0);
	rotation = new Vec(0,0,0);
	color = _color.clone();

	for (std::vector<Vec*>::size_type i = 0 ; i < _points.size(); i++ ) {
		addPoint(_points.at(i));
	}

	setCenter(calcCenter());
		
	//normal
	Vec cur =  rotate_xyz(*initpoints.at(1) - *position);
	Vec prev = rotate_xyz(*initpoints.at(0) - *position);
	Vec next = rotate_xyz(*initpoints.at(2) - *position);
	prev = prev - cur; 
	next = next - cur; 
	normal = prev.cross(next).normalize().clone();
}

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

/*
* set the position where the (rotation-)center of
* the polygon is located
*/
void Poly::setCenter(Vec c) {
	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);

	if ( initpoints.size() == 0 ) {
		return s;
	}

	for ( std::vector<Vec*>::size_type i = 0; i < initpoints.size(); i++ ) {
		s = s + *(initpoints.at(i));
	}

	return s / (float)initpoints.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();

	for ( std::vector<Vec*>::size_type i = 0; i < initpoints.size(); i++ ) {
		initpoints.at(i)->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());
}

Vec Poly::rotate_xyz(Vec out) {
	Vec ret = rotate_x(out);
	ret = rotate_y(ret);
	ret = rotate_z(ret);
	return ret;
}

void Poly::draw() {
	Vec n = Vec(0.0f, 0.0f, 0.0f);

	std::vector<Vec*>::size_type vcount = initpoints.size();

	//save camera position
	glPushMatrix();

	//center poly
	glTranslatef(-position->x(), -position->y(), -position->z());

	//rotate poly
	glRotatef(rotation->x(), 1.0f, 0.0f, 0.0f);
	glRotatef(rotation->y(), 0.0f, 1.0f, 0.0f);
	glRotatef(rotation->z(), 0.0f, 0.0f, 1.0f);
	
	glBegin (GL_POLYGON);

	for ( std::vector<Vec*>::size_type i = 0; i < vcount; i++ ) {

		/* face normal */
		//glNormal3fv(normal->c);

		/* TODO: vertex normal */
		n.c[0] = 0.0f;
		n.c[1] = 0.0f;
		n.c[2] = 0.0f;

		n = n+*normal;

		for ( std::vector<SPoly*>::size_type j = 0; j < initpoints.at(i)->faces.size(); j++ ) {
			if ( ((Poly*)initpoints.at(i)->faces.at(j))->normal->angle(*normal) < delta ) {
				n = n+*(((Poly*)initpoints.at(i)->faces.at(j))->normal);
			}
		}

		glNormal3fv(n.c);

		color->drawColor();
		glVertex3fv((*initpoints.at(i) - *position).c);
	}

	glEnd();

	//restore camera position
	glPopMatrix();
}

int Poly::numPoints() {
	return (int)initpoints.size();
}

void Poly::vardump(std::string whitespace) {
	printf("%sPoly(%d points, center: [%f, %f, %f], rotation: [%f, %f, %f], color: [%f, %f, %f])\n",
		whitespace.c_str(), initpoints.size(), 
		position->x(), position->y(), position->z(),
		rotation->x(), rotation->y(), rotation->z(), 
		color->r, color->g, color->b
	);

	for (std::vector<Vec*>::size_type i = 0 ; i < initpoints.size(); i++ ) {
		initpoints.at(i)->vardump(whitespace+"	");
	}
}

void Poly::addPoint(Vec* p) {
	p->registerFace(this);
	initpoints.push_back(p);
}

void Poly::setColor(Color c) {
	color = c.clone();
}

Color* Poly::getColor() {
	return color;
}

void Poly::setDelta(float d) {
	delta = d;
}