#include "Salamander.h"
#include "Shape.h"

#include "Line.h"
#include "World.h"

namespace Salamander
{

Shape::Shape()
{
	vworld_.clear();
	vlocal_.clear();
	edges_.clear();
	normals_.clear();
}

Shape::~Shape()
{
	vworld_.clear();
	vlocal_.clear();
	edges_.clear();
	normals_.clear();
}

bool Shape::addVertex(Vec2 vertex)
{
	// if we're just starting, then we can just add the vertex
	if(vworld_.size() < 2)
	{
		vlocal_.push_back(vertex);
		applyMatrix(Mat2());
		return true;
	}
	else
	{
		// must be convex
		if(isPointInside(vertex))	
			return false;

		// todo : insert vertex in the correct order

		vlocal_.push_back(vertex);
		applyMatrix(Mat2());
		generateProperties();
		return true;

	}
}

Vec2 Shape::getVertex(unsigned int i)
{
	return vworld_.at(i);
}

Vec2 Shape::getLocalVertex(unsigned int i)
{
	return vlocal_.at(i);
}

void Shape::setLocalVertex(unsigned int i, Vec2 position)
{
	vlocal_.at(i) = position;
	applyMatrix(Mat2());
	generateProperties();
}

void Shape::removeVertex(unsigned int i)
{
	vlocal_.erase(vlocal_.begin() + i);
	applyMatrix(Mat2());
	generateProperties();
}

std::vector<Vec2> Shape::getVertices()
{
	return vworld_;
}

unsigned int Shape::getNumVertices()
{
	return vworld_.size();
}

Vec2 Shape::normal(unsigned int i)
{
	return normals_.at(i);
}

Vec2 Shape::edge(unsigned int i)
{
	return edges_.at(i);
}

double Shape::radius()
{
	return radius_;
}

double Shape::radiusSquared()
{
	return radiusSquared_;
}

double Shape::momentOfInertia()
{
	return momentOfInertia_;
}

// is pretty much the same for all subclasses so implement here
Pick Shape::getRayIntersection(Vec2 location, Vec2 direction)
{
	// something to return
	Pick pick;

	unsigned int lines = vworld_.size();
	if(typeId() == Line::TypeId) lines--;

	// each visible line
	for(unsigned int i = 0; i < lines; i++)
	{
		Vec2 p1 = location;
		Vec2 e1 = direction;
		Vec2 p2 = vworld_.at(i);
		Vec2 e2 = edges_.at(i);
		Vec2 d = p2 - p1;

		double t = -1.0f;
		double s = -1.0f;

		double denominator = (e1.y_ * e2.x_ - e2.y_ * e1.x_);

		if(denominator != 0.0f)
		{
			// intersection point
			t = (d.y_ * e1.x_ - d.x_ * e1.y_) / denominator;
			s = (d.y_ * e2.x_ - d.x_ * e2.y_) / denominator;
		}

		// we only have an intersection if it lies within the line segment, and forwards of our location
		if(t >= 0.0f && t <= 1.0f && s >= 0.0f && s <= 1.0f)
		{
			// and only add it if it's the closest
			if(direction.length() * s < pick.distance || pick.distance == 0.0f)
			{
				pick.distance = direction.length() * s;
			}
		}
	}

	return pick;

}

// generate useful data that doesn't change between frames
void Shape::generateProperties()
{
	generateEdges();
	generateNormals();
	generateRadius();
}

// find the radius of the bounding circle
void Shape::generateRadius()
{
	radius_ = 0.0f;

	for(Vec2Iterator vertex = vlocal_.begin();
		vertex != vlocal_.end();
		++vertex)
	{
		double radius = (*vertex).length();
		if(radius > radius_) 
		{
			radius_ = radius;
			radiusSquared_ = radius * radius;
		}
	}
}

// create edge vectors from the vertices
void Shape::generateEdges()
{
	edges_.clear();
	for(unsigned int i = 0; i < vworld_.size(); i++)
	{
		edges_.push_back(vworld_[(i + 1) % vworld_.size()] - vworld_[i]);
	}
}

// create normals from the edge information
void Shape::generateNormals()
{
	normals_.clear();
	for(Vec2Iterator edge = edges_.begin();
		edge != edges_.end();
		++edge)
	{
		Vec2 normal = Vec2(-(*edge).y_, (*edge).x_);
		normal.normalise();
		normals_.push_back(normal);
	}
}

Vec2 Shape::generateCentroid()
{
	// just use the average of the points
	Vec2 centre;

	for(Vec2Iterator vertex = vlocal_.begin();
		vertex != vlocal_.end();
		++vertex)
	{
		centre += *vertex;
	}

	centre /= (double)vlocal_.size();

	// now adjust all of the points by that amount, and move the body by that amount
	for(Vec2Iterator vertex = vlocal_.begin();
		vertex != vlocal_.end();
		++vertex)
	{
		(*vertex) -= centre;
	}

	// update properties too
	generateProperties();
	clearMatrix();

	// so we can find sprite offsets and similar
	return centre;
}

// adjust the vertices from local to world space
void Shape::applyMatrix(Mat2 matrix)
{
	// ensure that the vectors match
	while(vworld_.size() < vlocal_.size()) {vworld_.push_back(Vec2());}
	while(vworld_.size() > vlocal_.size()) {vworld_.pop_back();}

	// apply a matrix to the cache vector and update the data in vertices
	for(unsigned int i = 0; i < vworld_.size(); i++)
	{
		vworld_.at(i) = matrix * vlocal_.at(i);
	}

	// update edges & normals
	generateEdges();
	generateNormals();
}

// return all vertices to local co-orditanes
void Shape::clearMatrix()
{
	applyMatrix(Mat2());
}

}
