//
//  Car.cpp
//  OSXGLEssentials
//
//  Created by Joshua Moerman on 10/03/14.
//
//

#include "Car.h"

#include <vector>
#include <random>
#include <functional>
#include <iostream>

static float get_rand(float min, float max){
    return min + (rand() / float(RAND_MAX)) * (max - min);
}

struct WheelGenome {
    float speed;
    float radius;
};

std::ostream & operator<<(std::ostream& out, WheelGenome const & g){
    return out << "(W speed=" << g.speed << " radius=" << g.radius << ")";
}

struct StickGenome {
    float angle;
    float limit;
    float width;
    float length;
    
    std::vector<WheelGenome> wheel;
    std::vector<StickGenome> sticks;
};

std::ostream & operator<<(std::ostream& out, StickGenome const & g){
    out << "(S angle=" << g.angle << " limit=" << g.limit << " width=" << g.width << " length=" << g.length;
    out << " wheel=";
    for(auto&& w : g.wheel) out << w;
    out << " sticks=";
    for(auto&& s : g.sticks) out << s;
    return out << ")";
}

struct CarGenome {
    std::vector<StickGenome> sticks;
};

std::ostream & operator<<(std::ostream& out, CarGenome const & g){
    out << "(C sticks=";
    for(auto&& s : g.sticks) out << s;
    return out << ")";
}

struct GenomeCreator {
    std::mt19937 generator{37};
    std::uniform_real_distribution<float> distribution_float{0.0, 1.0};
    std::uniform_int_distribution<int> distribution_int{0, 4};
    std::uniform_int_distribution<int> distribution_bool{0, 1};
    std::function<float()> r = std::bind(distribution_float, generator);
    std::function<int()> s = std::bind(distribution_int, generator);
    std::function<int()> w = std::bind(distribution_bool, generator);
    float min = 0.05f;
    
    WheelGenome wheel(){
        WheelGenome g;
        g.speed = r() * 20 + 10;
        g.radius = r() + min;
        return g;
    }
    
    StickGenome stick(){
        StickGenome g;
        g.angle = r() * 2 * M_PI;
        g.limit = r() * 0.05f * M_PI;
        g.width = r() * 0.2f + min;
        g.length = r() * 5 + 1;
        if(w()){
            g.wheel.push_back(wheel());
        } else {
            auto sticks = s() - 1;
            while(sticks-- > 0){
                g.sticks.push_back(stick());
            }
        }
        return g;
    }
    
    CarGenome car(){
        CarGenome g;
        auto sticks = s() + 1;
        while(sticks-- > 0){
            g.sticks.push_back(stick());
        }
        return g;
    }
};

static GenomeCreator creator;

struct CarBuilder {
    b2World & world;
    
    b2CircleShape wheelShape;
    b2FixtureDef wheelFixture;
    b2BodyDef wheelBody;
    
    b2PolygonShape stickShape;
    b2FixtureDef stickFixture;
    b2BodyDef stickBody;
    
    b2PolygonShape boxShape;
    b2FixtureDef boxFixture;
    b2BodyDef boxBody;
    
    b2RevoluteJointDef motor;
    b2RevoluteJointDef stickJoint;
    
    std::vector<b2Body*> created_parts;
    
    CarBuilder(b2World & world_)
    : world(world_)
    {
        // constants
        wheelFixture.shape = &wheelShape;
        wheelFixture.density = 1.0f;
        wheelFixture.friction = 1.0f;
        wheelFixture.restitution = 0.1f;
        wheelFixture.filter.groupIndex = -1;
        
        wheelBody.type = b2_dynamicBody;
        
        stickFixture.shape = &stickShape;
        stickFixture.density = 2.0f;
        stickFixture.friction = 0.7f;
        stickFixture.filter.groupIndex = -1;
        
        stickBody.type = b2_dynamicBody;
        
        boxShape.SetAsBox(0.05, 0.05);
        
        boxFixture.shape = &boxShape;
        boxFixture.density = 2000.0f;
        boxFixture.friction = 0.3f;
        boxFixture.filter.groupIndex = -1;
        
        boxBody.type = b2_dynamicBody;
        
        motor.maxMotorTorque = 100;
        motor.enableMotor = true;
        
        //stickJoint.enableLimit = true;
        stickJoint.enableMotor = true;
        stickJoint.maxMotorTorque = 1000;
    }
    
    b2Body * createWheel(WheelGenome const & g){
        wheelShape.m_radius = g.radius;
        
        auto body = world.CreateBody(&wheelBody);
        body->CreateFixture(&wheelFixture);
        created_parts.push_back(body);
        return body;
    }
    
    b2Body * createStick(StickGenome const & g){
        stickShape.SetAsBox(0.5 * g.length, 0.5 * g.width);
        
        auto body = world.CreateBody(&stickBody);
        body->CreateFixture(&stickFixture);
        created_parts.push_back(body);
        
        for(auto&& wg : g.wheel){
            auto wheel = createWheel(wg);
            wheel->SetTransform({g.length, 0.0}, 0);
            
            motor.localAnchorA = {0.5f * g.length, 0};
            motor.bodyA = body;
            motor.bodyB = wheel;
            motor.motorSpeed = -wg.speed; // CCW
            
            world.CreateJoint(&motor);
        }
        for(auto&& sg : g.sticks){
            auto stick = createStick(sg);
            float startAngle = sg.angle;
            stick->SetTransform({g.length, 0.0}, startAngle);
            
            stickJoint.localAnchorA = {0.5f * g.length, 0};
            stickJoint.localAnchorB = {-0.5f * sg.length, 0};
            stickJoint.bodyA = body;
            stickJoint.bodyB = stick;
            stickJoint.lowerAngle = startAngle - sg.limit;
            stickJoint.upperAngle = startAngle + sg.limit;
            stickJoint.referenceAngle = startAngle;
            stickJoint.motorSpeed = get_rand(-1, 2);
            
            world.CreateJoint(&stickJoint);
        }
        
        return body;
    }
    
    b2Body * createCar(CarGenome const & g){
        created_parts.clear();
        auto body = world.CreateBody(&boxBody);
        body->CreateFixture(&boxFixture);
        created_parts.push_back(body);
        
        for(auto&& sg : g.sticks){
            auto stick = createStick(sg);
            float startAngle = sg.angle;
            stick->SetTransform({sg.length, 0.0}, startAngle);
            
            stickJoint.localAnchorA = {0, 0};
            stickJoint.localAnchorB = {-0.5f * sg.length, 0};
            stickJoint.bodyA = body;
            stickJoint.bodyB = stick;
            stickJoint.lowerAngle = startAngle - sg.limit;
            stickJoint.upperAngle = startAngle + sg.limit;
            stickJoint.referenceAngle = startAngle;
            
            world.CreateJoint(&stickJoint);
        }
        
        return body;
    }
};

Car::Car(b2World & world_)
: world(world_)
{
    auto genome = creator.car();
    CarBuilder builder(world);
    
    body = builder.createCar(genome);
    parts.swap(builder.created_parts);
}

Car::Car(Car&& c)
: world(c.world)
, body(c.body)
, parts(std::move(c.parts))
{}

Car::~Car(){
    for(auto&& p : parts){
        world.DestroyBody(p);
    }
}

b2Vec2 Car::getPosition() const {
    return body->GetPosition();
}