#include "simul/robot.h"
#include <cmath>


#include "odometrie.h"
#include "asservissement.h"
#include "strategieV2.h"
#include "sensors.h"
#include <iostream>

#define ratio_qt_box2d 0.01

Odometrie* Odometrie::odometrie = NULL;

//Odometrie class implementation for the simulation
//Yes, it's ugly ! it should not be in this file.
//But in a separate file
Odometrie::Odometrie(Robot* robot) : robot(robot)
{
    Odometrie::odometrie = this;
}

PositionPlusAngle Odometrie::getPos() const
{
	return robot->getPos();
}

Distance Odometrie::getVitesseLineaire() const
{
	return robot->getVitesseLineaire();
}

Angle Odometrie::getVitesseAngulaire() const
{
	return robot->getVitesseAngulaire();
}

void Odometrie::setPos(const PositionPlusAngle& p)
{
	robot->setPos(p);
}

/////////

Robot::Robot(b2World & world, PositionPlusAngle depart, bool manual) : world(world), olds(10000)
{
    leftUpperHammerStatus = 0;
    leftLowerHammerStatus = 0;
    rightUpperHammerStatus = 0;
    rightLowerHammerStatus = 0;

    this->manual = manual;
	level = 0;

	odometrie = new Odometrie(this);

    odometrie->setPos(depart);
    pos = odometrie->getPos();
   //new Sensors();

    strategie = new StrategieV2(true);

	asservissement = new Asservissement(odometrie);




	//asservissement->strategie = strategie;

	deriv.position.x = 0;
	deriv.position.y = 0;
	deriv.angle = 0;

    /// Déclaration graphique (QT)
	int inc = 0;
	robotPolygonPoints[inc++] = QPoint(24,0);
	robotPolygonPoints[inc++] = QPoint(36,30);
	robotPolygonPoints[inc++] = QPoint(45,36);
	robotPolygonPoints[inc++] = QPoint(66,42);
	robotPolygonPoints[inc++] = QPoint(108,42);
	robotPolygonPoints[inc++] = QPoint(133,87);
	robotPolygonPoints[inc++] = QPoint(53,160);
	robotPolygonPoints[inc++] = QPoint(-25,160);
	robotPolygonPoints[inc++] = QPoint(-95,140);
	robotPolygonPoints[inc++] = QPoint(-95,-140);
	robotPolygonPoints[inc++] = QPoint(-25,-160);
	robotPolygonPoints[inc++] = QPoint(53,-160);
	robotPolygonPoints[inc++] = QPoint(133,-87);
	robotPolygonPoints[inc++] = QPoint(108,-42);
	robotPolygonPoints[inc++] = QPoint(66,-42);
	robotPolygonPoints[inc++] = QPoint(45,-36);
	robotPolygonPoints[inc++] = QPoint(36,-30);

    /// Déclaration Physique (Box2D)
	b2BodyDef bodyDef;
#ifndef BOX2D_2_0_1
	bodyDef.type = b2_dynamicBody;
#endif
	bodyDef.position.Set(pos.position.x/100., pos.position.y/100.);
	bodyDef.angle = pos.angle;

	body = world.CreateBody(&bodyDef);


#ifdef BOX2D_2_0_1
	b2PolygonDef box;
	b2PolygonDef &fixture = box;
#define CreateFixture CreateShape
#else
	b2PolygonShape box;
	b2FixtureDef fixture;
	fixture.shape = &box;
#endif
	fixture.density = 10.0f;
	fixture.friction = 1.0f;

    // On crée la "box" de base du robot : void b2PolygonShape::SetAsBox(float32 hx,float32 hy,const b2Vec2 & center,float32 angle)
	box.SetAsBox(60*ratio_qt_box2d,42*ratio_qt_box2d,b2Vec2((24.f-60.f)*ratio_qt_box2d,0),0);
	body->CreateFixture(&fixture);

#ifdef BOX2D_2_0_1
	b2Vec2* v = box.vertices;
    box.vertexCount = 6;
#else
    b2Vec2 v[6];
#endif
    // on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
	inc = 0;
	v[inc++].Set(-95.f*ratio_qt_box2d,42.f*ratio_qt_box2d);
	v[inc++].Set(108.f*ratio_qt_box2d,42.f*ratio_qt_box2d);
	v[inc++].Set(133.f*ratio_qt_box2d,87.f*ratio_qt_box2d);
	v[inc++].Set(53.f*ratio_qt_box2d,160.f*ratio_qt_box2d);
	v[inc++].Set(-25.f*ratio_qt_box2d,160.f*ratio_qt_box2d);
	v[inc++].Set(-95.f*ratio_qt_box2d,140.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
	box.Set(v, 6);
#endif
	body->CreateFixture(&fixture);

    // on déclare les points de la deuxieme partie : Attention, doit être convexe et orientée dans le sens indirect
	inc = 0;
	v[inc++].Set(-95.f*ratio_qt_box2d,-140.f*ratio_qt_box2d);
	v[inc++].Set(-25.f*ratio_qt_box2d,-160.f*ratio_qt_box2d);
	v[inc++].Set(53.f*ratio_qt_box2d,-160.f*ratio_qt_box2d);
	v[inc++].Set(133.f*ratio_qt_box2d,-87.f*ratio_qt_box2d);
	v[inc++].Set(108.f*ratio_qt_box2d,-42.f*ratio_qt_box2d);
	v[inc++].Set(-95.f*ratio_qt_box2d,-42.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
	box.Set(v, 6);
#endif
	body->CreateFixture(&fixture);

#ifdef BOX2D_2_0_1
	body->SetMassFromShapes();
#endif

	//Little hack so that linear and angular speed of the object
	//are those of the local coord (0,0) of the robot.
	//We don't really care of the mass center accuracy.
	b2MassData md;
	body->GetMassData(&md);
	md.center = b2Vec2(0,0);
	body->SetMassData(&md);
}

Robot::~Robot()
{
    delete asservissement;
    delete odometrie;
    delete strategie;
    world.DestroyBody(body);
}

void Robot::updateForces(int dt)
{
	if(dt == 0)
		return;

	Position impulse;
	impulse.x = (deriv.position.x*(float)cos(pos.angle) - deriv.position.y*(float)sin(pos.angle));
	impulse.y = (deriv.position.x*(float)sin(pos.angle) + deriv.position.y*(float)cos(pos.angle));

	float32 rdt = 1000./(float)dt;

	b2Vec2 bvelocity = 0.01*rdt*b2Vec2(impulse.x,impulse.y);
	float bangular = deriv.angle*rdt;
	//body->ApplyForce(10*body->GetMass()*(bimpulse - body->GetLinearVelocity()), body->GetWorldCenter());
	//body->ApplyTorque((bangular - body->GetAngularVelocity())*body->GetInertia());

	body->SetLinearVelocity(bvelocity);
	body->SetAngularVelocity(bangular);

}


void Robot::paint(QPainter &p, int dt)
{
	if(dt)
	{
		pos.position.x = 100*body->GetPosition().x;
		pos.position.y = 100*body->GetPosition().y;
		pos.angle = body->GetAngle();

		float rdt = (float)dt/1000.;
		deriv.angle = body->GetAngularVelocity()*rdt;
		float derx = 100*body->GetLinearVelocity().x*rdt;
		float dery = 100*body->GetLinearVelocity().y*rdt;

		deriv.position.x = derx*cos(pos.angle) + dery*sin(pos.angle);
		deriv.position.y = 0;

		//olds.push_back(pos);
		if(manual)
		{
			keyPressEvent(NULL,false);
			deriv.position.x = deriv.position.x* 0.97f;
			deriv.angle = deriv.angle * 0.9;
		}
		else
		{
            Odometrie::odometrie->update();
            StrategieV2::update();
            Asservissement::asservissement->update();
			deriv.position.x = asservissement->getLinearSpeed();
			deriv.position.y = 0;
			deriv.angle = asservissement->getAngularSpeed();
		}

        // hammers update

        if (leftLowerHammerStatus == 100)
            leftLowerHammerStatus = 0;
        if (rightLowerHammerStatus == 100)
            rightLowerHammerStatus = 0;
        if (leftUpperHammerStatus == 200)
            leftUpperHammerStatus = 0;
        if (rightUpperHammerStatus == 200)
            rightUpperHammerStatus = 0;

        if (leftLowerHammerStatus > 0)
            leftLowerHammerStatus += (int)(dt/2);
        if (rightLowerHammerStatus > 0)
            rightLowerHammerStatus += (int)(dt/2);
        if (leftUpperHammerStatus > 0)
            leftUpperHammerStatus += (int)(dt);
        if (rightUpperHammerStatus > 0)
            rightUpperHammerStatus += (int)(dt);

        if (leftLowerHammerStatus > 100)
            leftLowerHammerStatus = 100;
        if (rightLowerHammerStatus > 100)
            rightLowerHammerStatus = 100;
        if (leftUpperHammerStatus > 200)
            leftUpperHammerStatus = 200;
        if (rightUpperHammerStatus > 200)
            rightUpperHammerStatus = 200;

	}

    p.setWorldTransform(QTransform().translate(pos.position.x,-pos.position.y).rotateRadians(-pos.angle));


	p.setPen(QColor(Qt::black));
	p.setBrush(QBrush(QColor(90,90,90)));
	p.setOpacity(.3);

    // on peint le robot.
	p.drawConvexPolygon(robotPolygonPoints, 17);
    p.drawRect(-20, 160, 20, rightUpperHammerStatus);
    p.drawRect(0, 160, 20, rightLowerHammerStatus);
    p.drawRect(-20, -160, 20, -leftUpperHammerStatus);
    p.drawRect(0, -160, 20, -leftLowerHammerStatus);

//	p.drawChord(-103/2 + 104, -107, 2*103, 215, 16*90, 16*180);
	//p.drawRect(-268, -179.5, 268, 359);
	//drawTriangle(p, 0, 0,  65, 0,  60, 0);
	p.setOpacity(1);

	p.setPen(QColor(Qt::red));
	p.drawLine(0,0,pos.position.x,0);
	p.drawLine(0,100*pos.angle,0,0);
	p.setWorldTransform(QTransform());


	p.setPen(QColor(Qt::green));
	for(unsigned int i=0; i+1 < olds.size(); i++)
		p.drawLine(olds[i].position.x, -olds[i].position.y, olds[i+1].position.x, -olds[i+1].position.y);
}

#define IF_KEYSWITCH(n,a) \
static bool n=false; \
if(evt) n= a ? press : n; if(n)

void Robot::keyPressEvent(QKeyEvent* evt, bool press)
{
	if(evt && press && evt->text() == "e" && !evt->isAutoRepeat())
		manual = !manual;


	if(evt && press && evt->text() == "u" && !evt->isAutoRepeat())
	{
		level = (level != 100) ? 100 : 0;
#ifdef BOX2D_2_0_1
#define b2Filter b2FilterData
#define GetFixtureList GetShapeList
#endif
		b2Filter filter;
		if(level == 100)
		{
			filter.categoryBits = 0x4;
			filter.maskBits = 0x3;
		}
		else
		{
			filter.maskBits = 0x3;
			filter.categoryBits = 0x1;
		}
	}


	if(manual)
	{
        float dinc = .25;
        float ainc = 0.0025;

		IF_KEYSWITCH(avant,evt->key() == Qt::Key_Up)
			deriv.position.x += dinc;
		IF_KEYSWITCH(arriere,evt->key() == Qt::Key_Down)
			deriv.position.x -= dinc;
		IF_KEYSWITCH(gauche,evt->key() == Qt::Key_Right)
			deriv.angle -= ainc;
		IF_KEYSWITCH(droite,evt->key() == Qt::Key_Left)
			deriv.angle += ainc;
        IF_KEYSWITCH(leftUpperHammerUp,evt->key() == Qt::Key_W)
            leftUpperHammerStatus = 1;
        IF_KEYSWITCH(leftUpperHammerDown,evt->key() == Qt::Key_X)
            leftLowerHammerStatus = 1;
        IF_KEYSWITCH(rightUpperHammerUp,evt->key() == Qt::Key_C)
            rightUpperHammerStatus = 1;
        IF_KEYSWITCH(rightUpperHammerDown,evt->key() == Qt::Key_V)
            rightLowerHammerStatus = 1;

    }
    else // automated robot
    {
        IF_KEYSWITCH(leftUpperHammerUp,evt->key() == Qt::Key_O)
            StrategieV2::willCollide();
    }
}

void Robot::setLevel()
{
    		level = (level != 100) ? 100 : 0;
#ifdef BOX2D_2_0_1
#define b2Filter b2FilterData
#define GetFixtureList GetShapeList
#endif
		b2Filter filter;
		if(level == 100)
		{
			filter.categoryBits = 0x4;
			filter.maskBits = 0x3;
		}
		else
		{
			filter.maskBits = 0x3;
			filter.categoryBits = 0x1;

		}
}

PositionPlusAngle Robot::getPos()
{
    float alea1 = 0., alea2 = 0., alea3 = 0.;
 /*   alea1 = 2.1*(rand() % 801 -400)/1000;
    alea2 = 2.1*(rand() % 801 -400)/1000;
    alea3 = 2.1*(rand() % 801 -400)/6000; */
    PositionPlusAngle erreur(Position(alea1,alea2),Angle(alea3));
    return (pos + erreur);
}

void Robot::setPos(PositionPlusAngle p)
{
    pos = p;
    return;
}

Angle Robot::getVitesseAngulaire()
{
    return deriv.angle;
}

Distance Robot::getVitesseLineaire()
{
    return deriv.position.x;
}

QPoint Robot::getLeftUpperHammerPos() const
{
    return QPoint((this->pos.position.x - (160 + leftUpperHammerStatus - 10)*sin(pos.angle)), -(this->pos.position.y + (160 + leftUpperHammerStatus-10)*cos(pos.angle)));
}
QPoint Robot::getRightUpperHammerPos() const
{
    return QPoint(this->pos.position.x + (160 + rightUpperHammerStatus -10)*sin(pos.angle), -(this->pos.position.y - (160 + rightUpperHammerStatus-10)*cos(pos.angle)));
}
QPoint Robot::getLeftLowerHammerPos() const
{
    return QPoint(this->pos.position.x - (160 + leftLowerHammerStatus-10)*sin(pos.angle) , -(this->pos.position.y + (160 + leftLowerHammerStatus-10)*cos(pos.angle)));
}
QPoint Robot::getRightLowerHammerPos() const
{
    return QPoint(this->pos.position.x + (160 + rightLowerHammerStatus-10)*sin(pos.angle), -(this->pos.position.y - (160 + rightLowerHammerStatus-10)*cos(pos.angle)));
}
void Robot::startLeftUpperHammer()
{
    leftUpperHammerStatus = 1;
}
void Robot::startRightUpperHammer()
{
    rightUpperHammerStatus = 1;
}
void Robot::startLeftLowerHammer()
{
    leftLowerHammerStatus = 1;
}
void Robot::startRightLowerHammer()
{
    leftUpperHammerStatus = 1;
}