#include "vehicle.hpp"

#include "assets/cache.hpp"
#include "net/utils.hpp"
#include "player.hpp"
#include "player_input.hpp"

#include <iostream>

static std::shared_ptr<const assets::VehicleModel> LoadVehicleModelByName(const std::string& model_name)
{
    return assets::CacheManager::GetVehicleModel("data/" + model_name + ".veh");
}

struct Shape
{
    btBoxShape box;
    btCompoundShape compound;

    Shape() : box(btVector3(1, 1, 0.1))
    {
        btTransform t(btQuaternion(0, 0, 0), btVector3(0, 0, 2));
        compound.addChildShape(t, &box);
    }
};

game::Vehicle::Vehicle(World& world, std::string model_name)
    : Entity(world, net::ET_VEHICLE), model_name_(model_name), model_(LoadVehicleModelByName(model_name)),
      motion_(root_.local)
{
    root_.local.position.z = 10.0f;

    // setup chassis rigidbody
    float mass = 1300.0f;
    static Shape shape;

    btVector3 local_inertia(0, 0, 0);
    shape.compound.calculateLocalInertia(mass, local_inertia);

    btRigidBody::btRigidBodyConstructionInfo rb_info(mass, &motion_, &shape.compound, local_inertia);
    body_ = std::make_unique<btRigidBody>(rb_info);
    body_->setActivationState(DISABLE_DEACTIVATION);

    // setup vehicle
    btRaycastVehicle::btVehicleTuning tuning;
    vehicle_ = std::make_unique<btRaycastVehicle>(tuning, body_.get(), &world_.GetVehicleRaycaster());
    vehicle_->setCoordinateSystem(0, 2, 1);

    // setup wheels
    // btVector3 wheelDirectionCS0(0, -1, 0);
    // btVector3 wheelAxleCS(-1, 0, 0);
    btVector3 wheelDirectionCS0(0, 0, -1);
    btVector3 wheelAxleCS(1, 0, 0);

    wheel_z_offset_ = 0.4f;

    const auto& wheels = model_->GetWheels();

    if (wheels.size() > MAX_WHEELS)
        throw std::runtime_error("Max wheels exceeded");

    num_wheels_ = wheels.size();

    for (const auto& wheeldef : wheels)
    {
        float wheelRadius = .35f;

        float friction = 5.0f;
        float suspensionStiffness = 60.0f;
        // float suspensionDamping = 2.3f;
        // float suspensionCompression = 4.4f;
        float suspensionRestLength = 0.6f;
        float rollInfluence = 0.01f;

        float maxSuspensionForce = 100000.0f;
        float maxSuspensionTravelCm = 5000.0f;

        float k = 0.2;

        const bool is_front = !(wheeldef.type & assets::WHEEL_REAR);

        btVector3 wheel_pos(wheeldef.position.x, wheeldef.position.y, wheeldef.position.z + wheel_z_offset_);
        auto& wi = vehicle_->addWheel(wheel_pos, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius,
                                      tuning, is_front);

        wi.m_suspensionStiffness = suspensionStiffness;

        wi.m_wheelsDampingCompression = k * 2.0 * btSqrt(suspensionStiffness); // vehicleTuning.suspensionCompression;
        wi.m_wheelsDampingRelaxation = k * 3.3 * btSqrt(suspensionStiffness);  // vehicleTuning.suspensionDamping;

        wi.m_frictionSlip = friction;
        // if (wi.m_bIsFrontWheel) wi.m_frictionSlip = vehicleTuning.friction * 1.4f;

        wi.m_rollInfluence = rollInfluence;
        wi.m_maxSuspensionForce = maxSuspensionForce;
        wi.m_maxSuspensionTravelCm = maxSuspensionTravelCm;
    }

    auto& bt_world = world_.GetBtWorld();
    bt_world.addRigidBody(body_.get());
    bt_world.addAction(vehicle_.get());
}

void game::Vehicle::Update()
{
    Super::Update();

    ProcessInput();
    UpdateWheels();

    SendUpdateMsg();
}

void game::Vehicle::SendInitData(Player& player, net::OutMessage& msg) const
{
    net::ModelName name(model_name_);
    msg.Write(name);
}

game::Vehicle::~Vehicle()
{
    auto& bt_world = world_.GetBtWorld();
    bt_world.removeRigidBody(body_.get());
    bt_world.removeAction(vehicle_.get());
}

void game::Vehicle::ProcessInput()
{
    // TODO: totally fix

    float steeringIncrement = .04 * 60;
    // float steeringClamp = .5;
    float maxEngineForce = 7000;
    float maxBreakingForce = 300;

    float engineForce = 0;
    float breakingForce = 0;

    // process input
    PlayerInputFlags in = GetController() ? GetController()->GetInput() : 0;

    if (in & IN_DEBUG1)
    {
        auto t = body_->getWorldTransform();
        t.setOrigin(btVector3(100, 100, 5));
        body_->setWorldTransform(t);
    }

    float speed = vehicle_->getCurrentSpeedKmHour();

    float maxsc = .5f;
    float minsc = .08f;
    float sl = 130.f;

    float steeringClamp = std::max(minsc, (1.f - (std::abs(speed) / sl)) * maxsc);
    // steeringClamp = .5f;

    float t_delta = 1.0f / 25.0f;

    if (in & IN_FORWARD)
    {
        if (speed < -1)
            breakingForce = maxBreakingForce;
        else
            engineForce = maxEngineForce;
    }
    if (in & IN_BACKWARD)
    {
        if (speed > 1)
            breakingForce = maxBreakingForce;
        else
            engineForce = -maxEngineForce / 2;
    }

    // idle breaking
    if (!(in & IN_FORWARD) && !(in & IN_BACKWARD))
    {
        breakingForce = maxBreakingForce * 0.05f;
    }

    if (in & IN_LEFT)
    {
        if (steering_ < steeringClamp)
            steering_ += steeringIncrement * t_delta;
    }
    else
    {
        if (in & IN_RIGHT)
        {
            if (steering_ > -steeringClamp)
                steering_ -= steeringIncrement * t_delta;
        }
        else
        {
            if (steering_ < -steeringIncrement * t_delta)
                steering_ += steeringIncrement * t_delta;
            else
            {
                if (steering_ > steeringIncrement * t_delta)
                    steering_ -= steeringIncrement * t_delta;
                else
                {
                    steering_ = 0.0f;
                }
            }
        }
    }

    vehicle_->applyEngineForce(engineForce, 2);
    vehicle_->applyEngineForce(engineForce, 3);

    vehicle_->setBrake(breakingForce * 0.5, 0);
    vehicle_->setBrake(breakingForce * 0.5, 1);
    vehicle_->setBrake(breakingForce, 2);
    vehicle_->setBrake(breakingForce, 3);

    vehicle_->setSteeringValue(steering_, 0);
    vehicle_->setSteeringValue(steering_, 1);
}

void game::Vehicle::UpdateWheels()
{
    for (size_t i = 0; i < num_wheels_; ++i)
    {
        auto& bt_wheel = vehicle_->getWheelInfo(i);
        wheels_[i].speed = -(bt_wheel.m_rotation - wheels_[i].rotation) * 25.0f;
        wheels_[i].rotation = bt_wheel.m_rotation;
        wheels_[i].z_offset = wheel_z_offset_ - bt_wheel.m_raycastInfo.m_suspensionLength;
    }
}

void game::Vehicle::SendUpdateMsg()
{
    auto msg = BeginEntMsg(net::EMSG_UPDATE);
    net::WriteTransform(msg, root_.local);

    // send wheel info
    // msg.Write<uint8_t>(static_cast<uint8_t>(numwheels));
    msg.Write<net::AngleQ>(steering_);

    for (size_t i = 0; i < num_wheels_; ++i)
    {
        auto& wheel = wheels_[i];
        msg.Write<net::WheelZOffsetQ>(wheel.z_offset);
        msg.Write<net::RotationSpeedQ>(wheel.speed);
    }

    // TEMP wheels
    // TODO: REMOVE
    // for (size_t i =0; i < vehicle_->getNumWheels(); ++i)
    // {
    //     vehicle_->updateWheelTransform(i, true);
    //     btTransform tr = vehicle_->getWheelTransformWS(i);

    //     Transform trans;
    //     trans.SetBtTransform(tr);

    //     net::WriteTransform(msg, trans);

    //     // static glm::vec3 min_angles(1000.0f);
    //     // static glm::vec3 max_angles(-1000.0f);
    //     // min_angles = glm::min(min_angles, angles);
    //     // max_angles= glm::max(max_angles, angles);

    //     // std::cout << angles.x << " " << angles.y << " " << angles.z << " | " <<std::endl;
    // }
}