# include "Car.h"

Car::Car()
:m_imgCar( 0x0 ), m_imgWheel( 0x0 ),
m_bodyCar( 0x0 ),
m_cmd( FREE ),
m_currentTorque( 0.0f ), m_currentSpeed( 0.0f ), m_sense( 1.0f ), m_latence( 40.0f ),
m_goTorque( 0.9f ), m_minTorque( 0.0025f ), m_maxSpeed( 60.0f )
{
	// Constructeur
	m_currentTorque = m_minTorque ;
	m_currentSpeed = 0.0f ;
}

Car::~Car()
{
	if ( !m_imgCar )
		SDL_FreeSurface( m_imgCar );

	if ( !m_imgWheel )
		SDL_FreeSurface( m_imgWheel );
}

void Car::destroy( b2World &world )
{
    /// Détruire les liaisons
    // Les moteurs
    while ( !m_motorAxe.empty() )
    {
        world.DestroyJoint( m_motorAxe.back() );
        m_motorAxe.back() = nullptr;
        m_motorAxe.pop_back();
    }

    // Les roues libres
    while ( !m_fwdAxe.empty() )
    {
        world.DestroyJoint( m_fwdAxe.back() );
        m_fwdAxe.back() = nullptr;
        m_fwdAxe.pop_back();
    }

    /// Détruire les entités
    // Les roues
    while ( !m_bodyWheel.empty() )
    {
        world.DestroyBody( m_bodyWheel.back() );
        m_bodyWheel.back() = nullptr;
        m_bodyWheel.pop_back();
    }

    // Le carénage
    if ( m_bodyCar )
        world.DestroyBody( m_bodyCar );
    m_bodyCar = nullptr;
}

void Car::drive( command cmd )
{
     // Commande
	m_cmd = cmd ;

	// Actions
	if ( m_cmd == FREE )
	{
		m_currentSpeed = 0.0f ;
		m_currentTorque = m_minTorque ;
	}

	else if ( m_cmd == BREAK )
	{
		m_currentSpeed = 0.0f ;
		m_currentTorque = m_goTorque ;
	}

	else if ( m_cmd == GO )
	{
        m_sense = -1.0f ;
		m_currentSpeed = m_maxSpeed ;
		m_currentTorque = m_goTorque / 20.0f ;
	}

	else if ( m_cmd == REVERSE )
	{
        m_sense = 0.5f ;
		m_currentSpeed = m_maxSpeed ;
		m_currentTorque = m_goTorque / 20.0f ;
	}
}

void Car::jump( float correction )
{
     // Jump
     b2Vec2 spring;
     b2Vec2 forcePoint;

     spring.Set( 0.0f, -1.7f );
     forcePoint.Set( 0.0f, 0.0f );

     m_bodyCar->ApplyLinearImpulse( spring, forcePoint, true );

     // Correction
     m_bodyCar->ApplyAngularImpulse( - m_bodyCar->GetAngle() * correction, true );
}

void Car::spin( float tq )
{
    // Couple sur le carénage
    m_bodyCar->ApplyAngularImpulse( tq, true );
}

void Car::update()
{
	// Update state
     if ( m_cmd == GO || m_cmd == REVERSE )
	{
		m_currentTorque = ( m_latence * m_currentTorque + m_goTorque ) / ( m_latence + 1.0f ) ;
	}

	// Update motor
	for ( unsigned int i(0); i < m_motorAxe.size() ; i++ )
	{
          m_motorAxe[i]->SetMotorSpeed( m_sense * m_currentSpeed );
          m_motorAxe[i]->SetMaxMotorTorque( m_currentTorque );
	}
}

b2Vec2 Car::GetPosition()
{
	return m_bodyCar->GetPosition();
}

b2Vec2 Car::GetVelocity()
{
	return m_bodyCar->GetLinearVelocity();
}

float Car::GetTorque()
{
	return m_currentTorque;
}

float Car::GetSpeed()
{
	return m_currentSpeed;
}

bool Car::GetIsOnGround()
{
    bool rep( false );

    for ( b2ContactEdge* ce( m_bodyWheel.front()->GetContactList() ); ce && !rep; ce = ce->next )
        rep = ce->contact->IsTouching() ;

    return rep;
}

void Car::createMotorWheel( b2World &world, b2Vec2 rel, float friction, float density )
{
     // Vérifie la présence d'image
     if ( m_imgWheel == 0x0 )
     {
          std::cout << "Car::createMotorWheel > pas d'image." << std::endl ;
          return ;
     }

     // Vérifie la présence de carénage
     if ( m_bodyCar == 0x0 )
     {
          std::cout << "Car::createMotorWheel > pas de carénage." << std::endl ;
          return ;
     }

     // Dimensions
     float rwheel;
     rwheel = (float)m_imgWheel->h / MULTI / 2 ;

     // Définition body
     b2BodyDef bodyDef;
     b2CircleShape dynamicCircle;
     b2FixtureDef fixtureDef;
     bodyDef.type = b2_dynamicBody;

     // Roue moteur
     bodyDef.userData.pointer = (uintptr_t) m_imgWheel ;
     bodyDef.position = m_bodyCar->GetPosition() + rel ;

     dynamicCircle.m_radius = rwheel ;

     fixtureDef.shape = &dynamicCircle;
     fixtureDef.density = density;
     fixtureDef.friction = friction;

     m_bodyWheel.push_back( world.CreateBody(&bodyDef) );
     m_bodyWheel.back()->CreateFixture(&fixtureDef);

     // Création pivot arrière
     b2RevoluteJointDef myJointDef;
     myJointDef.collideConnected = false ;

     myJointDef.Initialize( m_bodyWheel.back(), m_bodyCar, m_bodyWheel.back()->GetPosition() );

     myJointDef.maxMotorTorque = 0.05f;
     myJointDef.motorSpeed = 0.0f;
     myJointDef.enableMotor = true;

     m_motorAxe.push_back( (b2RevoluteJoint*)world.CreateJoint(&myJointDef) );

     // Fin
     return ;
}

void Car::createFreeWheel( b2World &world, b2Vec2 rel, float friction, float density )
{
     // Vérifie la présence d'image
     if ( m_imgWheel == 0x0 )
     {
          std::cout << "Car::createFreeWheel > pas d'image." << std::endl ;
          return ;
     }

     // Vérifie la présence de carénage
     if ( m_bodyCar == 0x0 )
     {
          std::cout << "Car::createFreeWheel > pas de carénage." << std::endl ;
          return ;
     }

     // Dimensions
     float rwheel;
     rwheel = (float)m_imgWheel->h / MULTI / 2 ;

     // Définition body
     b2BodyDef bodyDef;
     b2CircleShape dynamicCircle;
     b2FixtureDef fixtureDef;
     bodyDef.type = b2_dynamicBody;

     // Roue avant
     bodyDef.userData.pointer = (uintptr_t) m_imgWheel ;
     bodyDef.position = m_bodyCar->GetPosition() + rel ;

     dynamicCircle.m_radius = rwheel ;

     fixtureDef.shape = &dynamicCircle;
     fixtureDef.density = density;
     fixtureDef.friction = friction;

     m_bodyWheel.push_back( world.CreateBody(&bodyDef) );
     m_bodyWheel.back()->CreateFixture(&fixtureDef);

     // Création pivot avant
     b2RevoluteJointDef myJointDef;
     myJointDef.collideConnected = false ;

     myJointDef.Initialize( m_bodyWheel.back(), m_bodyCar, m_bodyWheel.back()->GetPosition() );

     m_fwdAxe.push_back( (b2RevoluteJoint*)world.CreateJoint(&myJointDef) );

     // Fin
     return ;
}

void Car::createCarenage( b2World &world, float x, float y, float friction, float density )
{
     // Vérifie si présence de carénage
     if ( m_bodyCar != 0x0 )
     {
          world.DestroyBody( m_bodyCar );
          m_bodyCar = 0x0 ;
     }

     // Dimensions
     float wtruck, htruck ;
     wtruck = (float)m_imgCar->w / MULTI / 2 ;
     htruck = (float)m_imgCar->h / MULTI / 2 ;

     // Définition body
     b2BodyDef bodyDef;
     b2PolygonShape dynamicBox;
     b2FixtureDef fixtureDef;
     bodyDef.type = b2_dynamicBody;

     // Carrosserie
	 bodyDef.userData.pointer = (uintptr_t) m_imgCar ;
     bodyDef.position.Set( x, y );

     dynamicBox.SetAsBox( wtruck, htruck );

     fixtureDef.shape = &dynamicBox;
     fixtureDef.density = density;
     fixtureDef.friction = friction;

     m_bodyCar = world.CreateBody( &bodyDef );
     m_bodyCar->CreateFixture( &fixtureDef );
}