#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;	

 }

 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);

 */