#include "demo.hpp"
#include <fge.hpp>
#include <ode/ode.h>

using namespace fge;

namespace fge
{
    template<>
    Format Format::arg<dVector3>(dVector3 const & val)
    {
        return this->arg(Format("({0},{1},{2})").arg(val[0]).arg(val[1]).arg(val[2]).value);
    }
}

glm::vec3 cPos;
float cPan, cTilt;

Collider * trimesh;

void demo_init()
{
    fge::Initialize3D();

    fge::InitializePhysics();

    auto level = std::make_shared<Mesh>(fge::LoadStaticMesh("terrain.mdl"));
    auto block = std::make_shared<Mesh>(fge::LoadStaticMesh("block.obj"));
    auto sphere = std::make_shared<Mesh>(fge::LoadStaticMesh("sphere.mdl"));


    Entity * ball = new Entity();
    AddComponent<Geometry3D>(ball, sphere);
    AddComponent<Transform3D>(ball)->position = glm::vec3(0, 150, 0);
    auto * mtl = AddComponent<Material>(ball);
    mtl->ambient = RGBA(0xFF0000);
    mtl->diffuse = RGBA(0x00FF00);
    mtl->specular = RGBA(0x0000FF);

    Entity * world = new Entity();
    AddComponent<Geometry3D>(world, level);
    AddComponent<Transform3D>(world);

    auto mesh = CreateTriMesh(*level);
    trimesh = AddComponent<Collider>(world, dCreateTriMesh(
        GetSpace(),
        mesh,
        nullptr,
        nullptr,
        nullptr));

    Entity * cube = new Entity();
    AddComponent<Geometry3D>(cube, block);
    AddComponent<Transform3D>(cube)->position = glm::vec3(0,20,0);
    AddComponent<Collider>(cube, dCreateBox, 2.0, 2.0, 2.0);
    auto body = AddComponent<RigidBody>(cube)->body.get();

    dMass mass;
    dMassSetBoxTotal(&mass, 10.0, 1.0, 1.0, 1.0);
    dBodySetMass(body, &mass);

    AddEventHandler(SDL_KEYDOWN, [=](SDL_Event const & ev)
    {
        if(ev.key.keysym.sym == SDLK_SPACE)
        {
            dBodyAddForce(body, 0, 10000, 0);
            dBodyAddTorque(body, 20, 30, 40);
        }
    });
    AddEventHandler(SDL_MOUSEMOTION, [](SDL_Event const & ev)
    {
        cPan  -= 0.005f * ev.motion.xrel;
        cTilt -= 0.005f * ev.motion.yrel;
    });
}

glm::vec3 getForward()
{
    return glm::vec3(
        glm::cos(cTilt) * glm::sin(cPan),
        glm::sin(cTilt),
        glm::cos(cTilt) * glm::cos(cPan));
}

void demo_update()
{
    glm::vec3 fwd = getForward();
    glm::vec3 right = glm::cross(fwd, glm::vec3(0,1,0));

    if(fge::IsPressed(SDLK_w))
        cPos += fwd;
    if(fge::IsPressed(SDLK_s))
        cPos -= fwd;
    if(fge::IsPressed(SDLK_d))
        cPos += right;
    if(fge::IsPressed(SDLK_a))
        cPos -= right;

    fge::UpdatePhysics();
}

void demo_render()
{
    glClearColor(0.0f, 0.0f, 0.5f, 1.0f);
    glClearDepth(1.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    auto view = glm::lookAt(
        cPos,
        cPos + getForward(),
        glm::vec3(0, 1, 0));
    auto proj = glm::perspectiveFov(
        radians(60.0f),
        1280.0f,
        720.0f,
        0.1f,
        10000.0f);

    fge::Render3D(view, proj);

    {
        std::vector<fge::Vertex> lines;
        auto mesh = trimesh->geom.get();

        int count = dGeomTriMeshGetTriangleCount(mesh);
        for(int i = 0; i < count; i++)
        {
            dVector3 a, b, c;
            dGeomTriMeshGetTriangle(mesh, i, &a, &b, &c);

            glm::vec3 _a(a[0], a[1], a[2]);
            glm::vec3 _b(b[0], b[1], b[2]);
            glm::vec3 _c(c[0], c[1], c[2]);

            lines.push_back(Vertex { _a, RGBA(0xFF0000) });
            lines.push_back(Vertex { _b, RGBA(0xFF0000) });
            lines.push_back(Vertex { _b, RGBA(0xFF0000) });
            lines.push_back(Vertex { _c, RGBA(0xFF0000) });
            lines.push_back(Vertex { _c, RGBA(0xFF0000) });
            lines.push_back(Vertex { _a, RGBA(0xFF0000) });
        }
        fge::RenderRaw3D(view, proj, GL_LINES, lines.data(), lines.size());
    }
}