#include "geometry.h"

#include <math.h>

#include "misc.h"

#include <QString>

#include <iostream>

using namespace std;

QRectF addBBox(QRectF r1, QRectF r2)

{	

	// Find smallest QRectF containing given rectangles

	QRectF n;

	// Set left border

	if (r1.left() <= r2.left() )

		n.setLeft(r1.left() );

	else

		n.setLeft(r2.left() );

	// Set top border		

	if (r1.top() <= r2.top() )

		n.setTop(r1.top() );

	else

		n.setTop(r2.top() );

	// Set right border

	if (r1.right() <= r2.right() )

		n.setRight(r2.right() );

	else

		n.setRight(r1.right() );

	// Set bottom 

	if (r1.bottom() <= r2.bottom() )

		n.setBottom(r2.bottom() );

	else

		n.setBottom(r1.bottom() );

	return n;

}

bool isInBox(const QPointF &p, const QRectF &box)

{

    if (p.x() >= box.left() && p.x() <= box.right()  

	&& p.y() <= box.bottom() && p.y() >= box.top() )

		return true;

    return false;	

}

qreal distance (const QPointF &p, const QPointF &q)

{

	return sqrt (p.x()*q.x() + p.y()*q.y());

}

Vector::Vector ():QPointF ()

{

}

Vector::Vector (const QPointF &p):QPointF (p)

{

}

Vector::Vector (qreal x, qreal y):QPointF (x,y)

{

}

//! Normalize vector

void Vector::normalize ()

{

	if (x()==0 && y()==0) return;

	qreal l=sqrt ( x()*x() + y()*y() );

	setX (x()/l);

	setY (y()/l);

}

//! Dot product of two vectors

qreal Vector::dotProduct (const QPointF &b)

{

	return x()*b.x() + y()*b.y();

}

void Vector::scale (const qreal &f)

{

	setX (x()*f);

	setY (y()*f);

}

void Vector::invert ()

{

	setX (-x());

	setY (-y());

}

QPointF Vector::toQPointF ()

{

	return QPointF (x(),y());

}

/*! Calculate the projection of a polygon on an axis

    and returns it as a [min, max] interval  */

ConvexPolygon::ConvexPolygon ()

{

}

ConvexPolygon::ConvexPolygon (QPolygonF p):QPolygonF (p)

{

}

void ConvexPolygon::calcCentroid() 

{

	// Calculate area and centroid

	// http://en.wikipedia.org/wiki/Centroid

	qreal cx,cy,p;

	cx=cy=0;

	_area=0;

	append (at(0));

	for (int i=0;i<size()-1;i++)

	{

		p=at(i).x() * at(i+1).y() - at(i+1).x() * at(i).y();

		_area+=p;

		cx+=(at(i).x()+at(i+1).x()) * p;

		cy+=(at(i).y()+at(i+1).y()) * p;

	}	

	pop_back();

	// area is negative if vertices ordered counterclockwise

	// (in mirrored graphicsview!)

	_area=_area/2;	

	p=_area*6;

	_centroid.setX (cx/p);

	_centroid.setY (cy/p);

}

QPointF ConvexPolygon::centroid() const

{

	return _centroid;

}

qreal ConvexPolygon::weight() const

{

	return _area;

}

std::string ConvexPolygon::toStdString()

{

	QString s ("(");

	for (int i=0;i<size();++i)

	{

		s+=QString("(%1,%2)").arg(at(i).x()).arg(at(i).y());

		if (i<size()-1) s+=",";

	}

	s+=")";	

	return s.toStdString();

}

Vector ConvexPolygon::at(const int &i) const

{

	return Vector (QPolygonF::at(i).x(),QPolygonF::at(i).y());

}

void ConvexPolygon::translate ( const Vector & offset )

{ translate (offset.x(),offset.y());}

void ConvexPolygon::translate ( qreal dx, qreal dy )

{

	QPolygonF::translate (dx,dy);

	_centroid=_centroid+QPointF (dx,dy);

}

void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max) 

{

    // To project a point on an axis use the dot product

	//cout << "Projecting on "<< axis<<endl;

    qreal d = axis.dotProduct(polygon.at(0));

    min = d;

    max = d;

    for (int i = 0; i < polygon.size(); i++) 

	{

        d= polygon.at(i).dotProduct (axis);

        if (d < min) 

            min = d;

        else 

            if (d> max) max = d;

	//	cout << "p="<<polygon.at(i)<<"  d="<<d<<"  (min, max)=("<<min<<","<<max<<")\n";	

    }

}

// Calculate the signed distance between [minA, maxA] and [minB, maxB]

// The distance will be negative if the intervals overlap

qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {

    if (minA < minB) {

        return minB - maxA;

    } else {

        return minA - maxB;

    }

}

/*!

 Check if polygon A is going to collide with polygon B.

 The last parameter is the *relative* velocity 

 of the polygons (i.e. velocityA - velocityB)

*/

PolygonCollisionResult polygonCollision(ConvexPolygon polygonA, 

                              ConvexPolygon polygonB, Vector velocity) 

{

    PolygonCollisionResult result;

    result.intersect = true;

    result.willIntersect = true;

    int edgeCountA = polygonA.size();

    int edgeCountB = polygonB.size();

    qreal minIntervalDistance = 1000000000;

    QPointF translationAxis;

    QPointF edge;

/*

	cout << "\nA: ";

	for (int k=0; k<edgeCountA;k++)

		cout <<polygonA.at(k);

	cout << "\nB: ";

	for (int k=0; k<edgeCountB;k++)

		cout <<polygonB.at(k);

	cout <<endl;	

*/		

    // Loop through all the edges of both polygons

	for (int i=0;i<edgeCountA + edgeCountB;i++)

	{

        if (i< edgeCountA) 

		{

			// Loop through polygon A

			if (i<edgeCountA-1)

				edge = QPointF (

					polygonA.at(i+1).x()-polygonA.at(i).x(), 

					polygonA.at(i+1).y()-polygonA.at(i).y());

			else		

				edge = QPointF (

					polygonA.at(0).x()-polygonA.at(i).x(), 

					polygonA.at(0).y()-polygonA.at(i).y());

        } else 

		{

			// Loop through polygon B

			if (i < edgeCountA +edgeCountB -1 )

				edge = QPointF (

					polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(), 

					polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());

			else	

				edge = QPointF (

					polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(), 

					polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());

		}

        // ===== 1. Find if the polygons are currently intersecting =====

        // Find the axis perpendicular to the current edge

        Vector axis (-edge.y(), edge.x());

        axis.normalize();

        // Find the projection of the polygon on the current axis

        qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;

        projectPolygon(axis, polygonA, minA, maxA);

        projectPolygon(axis, polygonB, minB, maxB);

        // Check if the polygon projections are currentlty intersecting

        qreal d = intervalDistance(minA, maxA, minB, maxB);

        if (d > 0) result.intersect = false;

       // ===== 2. Now find if the polygons *will* intersect =====

        // Project the velocity on the current axis

        qreal velocityProjection = axis.dotProduct(velocity);

        // Get the projection of polygon A during the movement

        if (velocityProjection < 0) 

            minA += velocityProjection;

        else 

            maxA += velocityProjection;

        // Do the same test as above for the new projection

        // d = intervalDistance(minA, maxA, minB, maxB);

        //if (d > 0) result.willIntersect = false;

		/*

		cout <<"   ";

		cout << "edge="<<edge<<"  ";

		cout <<"axis="<<axis<<"  ";

		cout <<"dA=("<<minA<<","<<maxA<<")  dB=("<<minB<<","<<maxB<<")";

		cout <<"  d="<<d<<"   ";

		//cout <<"minD="<<minIntervalDistance<<"  ";

		cout <<"int="<<result.intersect<<"  ";

		//cout <<"wint="<<result.willIntersect<<"  ";

		//cout <<"velProj="<<velocityProjection<<"  ";

		cout <<endl;

		*/

        if (result.intersect )// || result.willIntersect) 

		{

			// Check if the current interval distance is the minimum one. If so

			// store the interval distance and the current distance.  This will

			// be used to calculate the minimum translation vector

			if (d<0) d=-d;

			if (d < minIntervalDistance) {

				minIntervalDistance = d;

				//translationAxis = axis;

				//cout << "tAxix="<<translationAxis<<endl;

				//QPointF t = polygonA.Center - polygonB.Center;

				//QPointF t = polygonA.at(0) - polygonB.at(0);

				//if (dotProduct(t,translationAxis) < 0)

				//	translationAxis = -translationAxis;

			}

		}

    }

    // The minimum translation vector

    // can be used to push the polygons appart.

    if (result.willIntersect)

        result.minTranslation = 

               translationAxis * minIntervalDistance;

    return result;

}

/* The function can be used this way: 

   QPointF polygonATranslation = new QPointF();

*/   

/*

PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);

if (r.WillIntersect) 

  // Move the polygon by its velocity, then move

  // the polygons appart using the Minimum Translation Vector

  polygonATranslation = velocity + r.minTranslation;

else 

  // Just move the polygon by its velocity

  polygonATranslation = velocity;

polygonA.Offset(polygonATranslation);

*/