// clang-format off
// glad must be included before any other OpenGL libraries
#include <glad/gl.h>
// clang-format on

#include <GLFW/glfw3.h>
#include <unistd.h>

#include <Tracy.hpp>
#include <csignal>
#include <cstdio>
#include <iostream>

// clang-format off
#ifndef IMGUI_DEFINE_MATH_OPERATORS
#define IMGUI_DEFINE_MATH_OPERATORS
#endif
#include "backends/imgui_impl_glfw.h"
#include "backends/imgui_impl_opengl3.h"
#include "ImGuizmo.h"
#include "imgui_internal.h"
#include "imgui.h"
// clang-format on

#include "render_utils.h"
#include "simulator.h"
#include "srender.h"
#include "utils.h"

#define SCONVCOL_IMPLEMENTATION
#include "sconvcol.h"

GLFWwindow* gWindow = nullptr;
GuiInputState* gGuiInputState = nullptr;
CameraState* gCameraState = nullptr;

// Rendering
srndr* gRndr = nullptr;
srview* gView = nullptr;
srcmdbuf* gRndrCmds = nullptr;

simd4x4f gProjMatrix{
    simd4f_create(1.f, 0.f, 0.0, 0.f),
    simd4f_create(0.f, 1.f, 0.f, 0.f),
    simd4f_create(0.f, 0.f, 1.f, 0.f),
    simd4f_create(0.f, 0.f, 0.f, 1.f)};
simd4x4f gTransformMatrix{
    simd4f_create(1.0f, 0.f, 0.0, 0.f),
    simd4f_create(0.f, 1.0f, 0.f, 0.f),
    simd4f_create(0.f, 0.f, 1.0f, 0.f),
    simd4f_create(0.f, 0.f, 0.f, 1.f)};

vectorial_inline simd4x4f simd4x4f_scale(simd4f s) {
  return simd4x4f_create(
      simd4f_create(simd4f_get_x(s), 0.f, 0.f, 0.f),
      simd4f_create(0.f, simd4f_get_y(s), 0.f, 0.f),
      simd4f_create(0.f, 0.f, simd4f_get_z(s), 0.f),
      simd4f_create(0.f, 0.f, 0.f, simd4f_get_w(s)));
}

double mouse_scroll_x = 0.;
double mouse_scroll_y = 0.;

enum ViewMode { ViewModeSimulation = 0, ViewModeGuizmoTest = 1 };

ViewMode gViewMode = ViewModeSimulation;
bool gGizmoEnabled = true;

enum ControlMode { ControlModeNone = 0, ControlModeFirstPersonView = 1 };
ControlMode gControlMode = ControlModeNone;

struct Dialogs {
  bool mCameraDialog = false;
  bool mImGuiDemo = false;
  bool mSimulator = false;
  bool mImGuizmoTransform = false;
};
Dialogs* gDialogs = nullptr;

using namespace std;

static void error_callback(int error, const char* description) {
  fprintf(stderr, "Error (%d): %s\n", error, description);
}

static void opengl_error_callback(
    GLenum source,
    GLenum type,
    GLuint id,
    GLenum severity,
    GLsizei length,
    const GLchar* message,
    const void* userParam) {
  const char* source_str;
  switch (source) {
    case GL_DEBUG_SOURCE_API:
      source_str = "API";
      break;
    case GL_DEBUG_SOURCE_WINDOW_SYSTEM:
      source_str = "Window System";
      break;
    case GL_DEBUG_SOURCE_SHADER_COMPILER:
      source_str = "Shader Compiler";
      break;
    case GL_DEBUG_SOURCE_THIRD_PARTY:
      source_str = "3rd Party";
      break;
    case GL_DEBUG_SOURCE_APPLICATION:
      source_str = "Application";
      break;
    default:
      source_str = "Other";
      break;
  }

  const char* type_str;
  switch (type) {
    case GL_DEBUG_TYPE_ERROR:
      type_str = "Error";
      break;
    case GL_DEBUG_TYPE_DEPRECATED_BEHAVIOR:
      type_str = "Deprecated Behaviour";
      break;
    case GL_DEBUG_TYPE_UNDEFINED_BEHAVIOR:
      type_str = "Undefined Behaviour";
      break;
    case GL_DEBUG_TYPE_PORTABILITY:
      type_str = "Portability";
      break;
    case GL_DEBUG_TYPE_PERFORMANCE:
      type_str = "Performance";
      break;
    case GL_DEBUG_TYPE_MARKER:
      type_str = "Marker";
      break;
    case GL_DEBUG_TYPE_PUSH_GROUP:
      type_str = "Push Group";
      break;
    case GL_DEBUG_TYPE_POP_GROUP:
      type_str = "Pop Group";
      break;
    default:
      type_str = "Other";
      break;
  }

  const char* severity_str;
  switch (severity) {
    case GL_DEBUG_SEVERITY_HIGH:
      severity_str = "high";
      break;
    case GL_DEBUG_SEVERITY_MEDIUM:
      severity_str = "medium";
      break;
    case GL_DEBUG_SEVERITY_LOW:
      severity_str = "low";
      break;
    case GL_DEBUG_SEVERITY_NOTIFICATION:
      severity_str = "notification";
      break;
    default:
      severity_str = "unknown";
  }

  gLog(
      "OpenGL Error (%s, type %s, severity = %s): %s",
      source_str,
      type_str,
      severity_str,
      message);
}

static void
key_callback(GLFWwindow* window, int key, int scancode, int action, int mods) {
  if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS)
    glfwSetWindowShouldClose(window, GLFW_TRUE);
}

void signal_handler(int signo) { gLog("Received signal %d", signo); }

void mouse_scroll_callback(GLFWwindow* window, double xoffset, double yoffset) {
  mouse_scroll_x += xoffset;
  mouse_scroll_y += yoffset;
}

void handle_mouse() {
  if (!glfwGetWindowAttrib(gWindow, GLFW_FOCUSED)) {
    return;
  }

  double mouse_x, mouse_y;
  glfwGetCursorPos(gWindow, &mouse_x, &mouse_y);

  if (gGuiInputState->mouseButton) {
    gGuiInputState->mousedX = int32_t(mouse_x) - gGuiInputState->mouseX;
    gGuiInputState->mousedY = int32_t(mouse_y) - gGuiInputState->mouseY;
  } else {
    gGuiInputState->mousedX = 0;
    gGuiInputState->mousedY = 0;
  }
  gGuiInputState->mouseX = int32_t(mouse_x);
  gGuiInputState->mouseY = int32_t(mouse_y);
  gGuiInputState->mouseScroll = int32_t(mouse_scroll_y);

  gGuiInputState->mouseButton = glfwGetMouseButton(gWindow, 0)
                                + (glfwGetMouseButton(gWindow, 1) << 1)
                                + (glfwGetMouseButton(gWindow, 2) << 2);
}

void ShowDockspace(bool open) {
  static bool opt_fullscreen = true;
  static bool opt_padding = false;
  static ImGuiDockNodeFlags dockspace_flags = ImGuiDockNodeFlags_None;

  ImGuiIO& io = ImGui::GetIO();
  (void)io;

  // We are using the ImGuiWindowFlags_NoDocking flag to make the parent window not dockable into,
  // because it would be confusing to have two docking targets within each others.
  ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking;
  if (opt_fullscreen) {
    ImGuiViewport* viewport = ImGui::GetMainViewport();
    ImGui::SetNextWindowPos(viewport->GetWorkPos());
    ImGui::SetNextWindowSize(viewport->GetWorkSize());
    ImGui::SetNextWindowViewport(viewport->ID);
    ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f);
    ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f);
    window_flags |= ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse
                    | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove;
    window_flags |=
        ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus;
  } else {
    dockspace_flags &= ~ImGuiDockNodeFlags_PassthruCentralNode;
  }

  // When using ImGuiDockNodeFlags_PassthruCentralNode, DockSpace() will render our background
  // and handle the pass-thru hole, so we ask Begin() to not render a background.
  if (dockspace_flags & ImGuiDockNodeFlags_PassthruCentralNode)
    window_flags |= ImGuiWindowFlags_NoBackground;

  // Important: note that we proceed even if Begin() returns false (aka window is collapsed).
  // This is because we want to keep our DockSpace() active. If a DockSpace() is inactive,
  // all active windows docked into it will lose their parent and become undocked.
  // We cannot preserve the docking relationship between an active window and an inactive docking, otherwise
  // any change of dockspace/settings would lead to windows being stuck in limbo and never being visible.
  if (!opt_padding)
    ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0.0f, 0.0f));
  ImGui::Begin("DockSpace Demo", &open, window_flags);
  if (!opt_padding) ImGui::PopStyleVar();

  if (opt_fullscreen) ImGui::PopStyleVar(2);

  // DockSpace
  if (io.ConfigFlags & ImGuiConfigFlags_DockingEnable) {
    ImGuiID dockspace_id = ImGui::GetID("MyDockSpace");
    ImGui::DockSpace(dockspace_id, ImVec2(0.0f, 0.0f), dockspace_flags);
  } else {
    gLog("Error: no docking not enabled");
  }

  ImGui::End();
}

static ImGuizmo::OPERATION mCurrentGizmoOperation(ImGuizmo::TRANSLATE);

void RenderGuizmoTest(srcmdbuf* cmdbuf) {
  srcmd rcmd;
  srcmd_clear(&rcmd);

  rcmd.type = SRndrCmdTypeGrid;
  for (int i = -1; i <= 1; i++) {
    for (int j = -1; j <= 1; j++) {
      simd4x4f_translation(&rcmd.mat, 0.0f, 0.0f, 0.0f);
      srcmdbuf_add(cmdbuf, &rcmd);
    }
  }

  rcmd.type = SRndrCmdTypeCube;
  rcmd.mat = gTransformMatrix;
  srcmdbuf_add(cmdbuf, &rcmd);
}

void ShowViewManip() {
  const ImGuiStyle& imgui_style = ImGui::GetStyle();

  ImGuizmo::SetDrawlist();
  float windowWidth = (float)ImGui::GetWindowWidth();
  float windowHeight = (float)ImGui::GetWindowHeight();
  ImGuizmo::SetRect(
      ImGui::GetWindowPos().x,
      ImGui::GetWindowPos().y,
      windowWidth,
      windowHeight);
  float viewManipulateRight = ImGui::GetWindowPos().x + windowWidth;

  float title_height = ImGui::GetFontSize() + imgui_style.FramePadding.y * 2.f;
  ImVec2 window_pos = ImGui::GetWindowPos();

  ImVec2 view_manip_widget_size(128, 128);
  ImVec2 view_manip_widget_pos(
      viewManipulateRight - 128 - imgui_style.WindowPadding.x,
      window_pos.y + title_height + imgui_style.WindowPadding.y);
  ImGui::SetCursorPos(view_manip_widget_pos - ImGui::GetWindowPos());
  ImGui::InvisibleButton("ViewManipulator", view_manip_widget_size);
  ImGuizmo::ViewManipulate(
      (float*)&(gCameraState->mtxView.x),
      10.0f,
      view_manip_widget_pos,
      view_manip_widget_size,
      0x40404040);
}

void ShowTransformManip(
    float* cameraView,
    float* cameraProjection,
    float* matrix) {
  static ImGuizmo::MODE mCurrentGizmoMode(ImGuizmo::LOCAL);
  static bool useSnap = false;
  static float snap[3] = {1.f, 1.f, 1.f};
  static float bounds[] = {-0.5f, -0.5f, -0.5f, 0.5f, 0.5f, 0.5f};
  static float boundsSnap[] = {0.1f, 0.1f, 0.1f};
  static bool boundSizing = false;
  static bool boundSizingSnap = false;
  bool useWindow = false;
  bool editTransformDecomposition = true;

  ImVec2 window_size = ImGui::GetWindowSize();
  ImVec2 window_pos = ImGui::GetWindowPos();

  ImGuiStyle imgui_style = ImGui::GetStyle();
  float title_height = ImGui::GetFontSize() + imgui_style.FramePadding.y * 2.f;

  window_pos.x += imgui_style.WindowPadding.x;
  window_pos.y += imgui_style.WindowPadding.y + title_height;

  window_size.x -= imgui_style.WindowPadding.x * 2;
  window_size.y -= imgui_style.WindowPadding.y * 2 + title_height;

  if (gDialogs->mImGuizmoTransform) {
    ImGui::Begin("Guizmo", &gDialogs->mImGuizmoTransform);

    if (editTransformDecomposition) {
      if (ImGui::IsKeyPressed(90)) mCurrentGizmoOperation = ImGuizmo::TRANSLATE;
      if (ImGui::IsKeyPressed(69)) mCurrentGizmoOperation = ImGuizmo::ROTATE;
      if (ImGui::IsKeyPressed(82))  // r Key
        mCurrentGizmoOperation = ImGuizmo::SCALE;
      if (ImGui::RadioButton(
              "Translate",
              mCurrentGizmoOperation == ImGuizmo::TRANSLATE))
        mCurrentGizmoOperation = ImGuizmo::TRANSLATE;
      ImGui::SameLine();
      if (ImGui::RadioButton(
              "Rotate",
              mCurrentGizmoOperation == ImGuizmo::ROTATE))
        mCurrentGizmoOperation = ImGuizmo::ROTATE;
      ImGui::SameLine();
      if (ImGui::RadioButton(
              "Scale",
              mCurrentGizmoOperation == ImGuizmo::SCALE))
        mCurrentGizmoOperation = ImGuizmo::SCALE;
      if (ImGui::RadioButton(
              "Universal",
              mCurrentGizmoOperation == ImGuizmo::UNIVERSAL))
        mCurrentGizmoOperation = ImGuizmo::UNIVERSAL;
      float matrixTranslation[3], matrixRotation[3], matrixScale[3];
      ImGuizmo::DecomposeMatrixToComponents(
          matrix,
          matrixTranslation,
          matrixRotation,
          matrixScale);
      ImGui::InputFloat3("Tr", matrixTranslation);
      ImGui::InputFloat3("Rt", matrixRotation);
      ImGui::InputFloat3("Sc", matrixScale);
      ImGuizmo::RecomposeMatrixFromComponents(
          matrixTranslation,
          matrixRotation,
          matrixScale,
          matrix);

      if (mCurrentGizmoOperation != ImGuizmo::SCALE) {
        if (ImGui::RadioButton("Local", mCurrentGizmoMode == ImGuizmo::LOCAL))
          mCurrentGizmoMode = ImGuizmo::LOCAL;
        ImGui::SameLine();
        if (ImGui::RadioButton("World", mCurrentGizmoMode == ImGuizmo::WORLD))
          mCurrentGizmoMode = ImGuizmo::WORLD;
      }
      if (ImGui::IsKeyPressed(83)) useSnap = !useSnap;
      ImGui::Checkbox("", &useSnap);
      ImGui::SameLine();

      switch (mCurrentGizmoOperation) {
        case ImGuizmo::TRANSLATE:
          ImGui::InputFloat3("Snap", &snap[0]);
          break;
        case ImGuizmo::ROTATE:
          ImGui::InputFloat("Angle Snap", &snap[0]);
          break;
        case ImGuizmo::SCALE:
          ImGui::InputFloat("Scale Snap", &snap[0]);
          break;
        default:
          break;
      }
      ImGui::Checkbox("Bound Sizing", &boundSizing);
      if (boundSizing) {
        ImGui::PushID(3);
        ImGui::Checkbox("", &boundSizingSnap);
        ImGui::SameLine();
        ImGui::InputFloat3("Snap", boundsSnap);
        ImGui::PopID();
      }
    }

    ImGui::End();
  }

  ImGuiIO& io = ImGui::GetIO();
  float viewManipulateRight = io.DisplaySize.x;
  float viewManipulateTop = 0;
  if (useWindow) {
    ImGui::SetNextWindowSize(ImVec2(800, 400));
    ImGui::SetNextWindowPos(ImVec2(400, 20));
    ImGui::PushStyleColor(
        ImGuiCol_WindowBg,
        (ImVec4)ImColor(0.35f, 0.3f, 0.3f));
    ImGui::Begin("Gizmo", 0);
    ImGuizmo::SetDrawlist();
    float windowWidth = (float)ImGui::GetWindowWidth();
    ImGuizmo::SetRect(window_pos.x, window_pos.y, window_size.x, window_size.y);
  } else {
    ImGuizmo::SetDrawlist();
    ImGuizmo::SetRect(window_pos.x, window_pos.y, window_size.x, window_size.y);
  }

  simd4x4f identity_mat;
  simd4x4f_identity(&identity_mat);

  bool modified = ImGuizmo::Manipulate(
      cameraView,
      cameraProjection,
      mCurrentGizmoOperation,
      mCurrentGizmoMode,
      matrix,
      NULL,
      useSnap ? &snap[0] : NULL,
      boundSizing ? bounds : NULL,
      boundSizingSnap ? boundsSnap : NULL);

  if (useWindow) {
    ImGui::End();
    ImGui::PopStyleColor(1);
  }
}

void SimulatorSceneRender(srcmdbuf* render_commands) {
  // Populate render commands
  srcmdbuf_clear(render_commands);

  srcmd rcmd;
  srcmd_clear(&rcmd);

  rcmd.type = SRndrCmdTypeFrame;
  srcmdbuf_add(render_commands, &rcmd);

  simulator_draw(render_commands);

  if (gDialogs->mSimulator) {
    ImGui::Begin("Simulator", &gDialogs->mSimulator);
    simulator_gui();
    ImGui::End();
  }
}

struct SceneHull {
  sch_hull hull;
  srmeshdata* mesh = nullptr;
  simd4x4f transform = {
      simd4f_create(1.f, 0.f, 0.0, 0.f),
      simd4f_create(0.f, 1.f, 0.f, 0.f),
      simd4f_create(0.f, 0.f, 1.f, 0.f),
      simd4f_create(0.f, 0.f, 0.f, 1.f)};

  ~SceneHull() {
    if (mesh != nullptr) {
      destroy();
    }
  };

  void destroy() {
    srmesh_destroy(mesh);
    mesh = nullptr;
  }

  void createMesh() {
    assert(mesh == nullptr);

    // create mesh for hull
    static srvrtxdata mesh_buffer_data[1024];
    static GLuint index_buffer_data[1024];
    int vertex_index = -1;
    int index_count = -1;
    int triangle_count = 0;

    int face_vert0_idx = 0;

    for (int fi = 0; fi < hull.num_faces; fi++) {
      sch_face* face = &hull.faces[fi];
      sch_edge* edge0 = face->edge;
      sch_vert* face_vert0 = edge0->vert;
      sch_edge* cur_edge = edge0->next;
      sch_vert* cur_vert = nullptr;
      sch_edge* next_edge = nullptr;
      sch_vert* next_vert = nullptr;

      int tri_vert_0_idx = 0;
      int tri_vert_1_idx = 0;
      int tri_vert_2_idx = 0;

      sch_plane plane;
      sch_hull_calc_plane(&hull, fi, &plane);

      // add face_vert0 (later used as tri_vert_0)
      vertex_index++;
      face_vert0_idx = vertex_index;
      tri_vert_0_idx = face_vert0_idx;
      srvrtxdata* mesh_vert = &mesh_buffer_data[tri_vert_0_idx];
      mesh_vert->x = simd4f_get_x(face_vert0->p);
      mesh_vert->y = simd4f_get_y(face_vert0->p);
      mesh_vert->z = simd4f_get_z(face_vert0->p);
      mesh_vert->w = 1.f;
      mesh_vert->nx = simd4f_get_x(plane.n);
      mesh_vert->ny = simd4f_get_y(plane.n);
      mesh_vert->nz = simd4f_get_z(plane.n);
      mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
      mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
      mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
      mesh_vert->color[3] = 255;

      // add tri_vert_1
      cur_vert = cur_edge->vert;
      tri_vert_1_idx = ++vertex_index;
      mesh_vert = &mesh_buffer_data[tri_vert_1_idx];
      mesh_vert->x = simd4f_get_x(cur_vert->p);
      mesh_vert->y = simd4f_get_y(cur_vert->p);
      mesh_vert->z = simd4f_get_z(cur_vert->p);
      mesh_vert->w = 1.f;
      mesh_vert->nx = simd4f_get_x(plane.n);
      mesh_vert->ny = simd4f_get_y(plane.n);
      mesh_vert->nz = simd4f_get_z(plane.n);
      mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
      mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
      mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
      mesh_vert->color[3] = 255;

      do {
        // add face_vert0
        index_buffer_data[++index_count] = tri_vert_0_idx;

        // add face_vert1
        index_buffer_data[++index_count] = tri_vert_1_idx;

        next_edge = cur_edge->next;
        next_vert = next_edge->vert;
        tri_vert_2_idx = ++vertex_index;

        mesh_vert = &mesh_buffer_data[tri_vert_2_idx];
        mesh_vert->x = simd4f_get_x(next_vert->p);
        mesh_vert->y = simd4f_get_y(next_vert->p);
        mesh_vert->z = simd4f_get_z(next_vert->p);
        mesh_vert->w = 1.f;
        mesh_vert->nx = simd4f_get_x(plane.n);
        mesh_vert->ny = simd4f_get_y(plane.n);
        mesh_vert->nz = simd4f_get_z(plane.n);
        mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
        mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
        mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
        mesh_vert->color[3] = 255;
        index_buffer_data[++index_count] = tri_vert_2_idx;

        triangle_count++;
        cur_edge = next_edge;
        tri_vert_1_idx = tri_vert_2_idx;
      } while (next_edge->next != edge0);
    }

    mesh = srmesh_create(
        mesh_buffer_data,
        vertex_index + 1,
        index_buffer_data,
        index_count + 1,
        GL_TRIANGLES,
        index_count + 1);
  }
};

struct SconvHullScene {
  SceneHull mHullA;
  SceneHull mHullB;
  SceneHull* mCurrentHull;

  void setupSceneRotTrans() {
    simd4x4f rot;
    simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));

    simd4x4f translation;
    simd4x4f_translation(&translation, -0.5f  - 0.51f * sqrtf(2.), 0.0f, 0.f);

    simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
  }

  void setupSceneEdgeCollision() {
    simd4x4f rot_y;
    simd4x4f_axis_rotation(&rot_y, 0.25f * M_PI, simd4f_create (0.f, 1.f, 0.f, 1.f));

    simd4x4f rot_z;
    simd4x4f_axis_rotation(&rot_z, 0.25f * M_PI, simd4f_create (0.f, 0.f, 1.f, 1.f));

    simd4x4f rot;
    simd4x4f_matrix_mul(&rot_y, &rot_z, &rot);

    simd4x4f translation;
    simd4x4f_translation(&translation, 0.98f, 0.f, -0.98f);

    simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
  }

	void setupRotatedBoxes () {
		simd4x4f trans_A;
		simd4x4f trans_B;
		simd4x4f_axis_rotation (&trans_A, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));

		simd4x4f rot_B;
		simd4x4f_axis_rotation (&rot_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
		simd4x4f translation_B;
		simd4x4f_translation (&translation_B, sqrt(2.f) + 0.001, 0.f, 0.f);

		simd4x4f_matrix_mul (&translation_B, &rot_B, &trans_B);

		mHullA.transform = trans_A;
		mHullB.transform = trans_B;
	}

  void init() {
    sch_create_unitbox(&mHullA.hull);
    mHullA.createMesh();

    sch_create_unitbox(&mHullB.hull);
    mHullB.createMesh();

    // setupSceneEdgeCollision();
		setupRotatedBoxes();

    mCurrentHull = &mHullA;
  };

  void destroy() {
    mHullA.destroy();
    mHullB.destroy();
  }

  void draw(srcmdbuf* render_commands) {
    // Populate render commands
    srcmdbuf_clear(render_commands);

    srcmd rcmd;
    srcmd_clear(&rcmd);

    rcmd.type = SRndrCmdTypeFrame;
    srcmdbuf_add(render_commands, &rcmd);

    rcmd.type = SRndrCmdTypeGrid;
    float origin[3] = {0.f, 0.f, 0.f};

    for (int i = -1; i <= 1; i++) {
      for (int j = -1; j <= 1; j++) {
        simd4x4f_translation(
            &rcmd.mat,
            origin[0] + i * 10.0f,
            origin[1],
            origin[2] + j * 10.f);
        srcmdbuf_add(render_commands, &rcmd);
      }
    }

    ImGuizmo::Manipulate(
        (float*)&(gCameraState->mtxView),
        (float*)&(gProjMatrix),
        ImGuizmo::TRANSLATE,
        ImGuizmo::WORLD,
        (float*)(&mCurrentHull->transform),
        NULL);

    // Render hulls
    srcmd cmd;
    cmd.type = SrndrCmdTypeMesh;
    cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
    cmd.mat = mHullA.transform;
    cmd.meshdata = mHullA.mesh;

    srcmdbuf_add(render_commands, &cmd);

    cmd.type = SrndrCmdTypeMesh;
    cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
    cmd.mat = mHullB.transform;
    cmd.meshdata = mHullB.mesh;

    srcmdbuf_add(render_commands, &cmd);
  }
};

SconvHullScene gHullScene;

void ImSRenderWidget(srndr* renderer, srview* view) {
  ZoneScoped;

  const ImGuiStyle& imgui_style = ImGui::GetStyle();

  // Render Output
  ImGui::Begin("Render Output", nullptr, ImGuiWindowFlags_NoScrollbar);

  // Update the gCameraState->mtxView
  const ImVec2 content_avail = ImGui::GetContentRegionAvail();
  srview_set_size(view, int(content_avail.x), int(content_avail.y));

  float view_width = content_avail.x / 50.f;
  float view_height = content_avail.y / 50.f;

  simd4x4f_ortho(
      &gProjMatrix,
      -0.5f * view_width,
      0.5f * view_width,
      -0.5f * view_height,
      0.5f * view_height,
      -50.0f,
      50.0f);
  simd4x4f_perspective(
      &gProjMatrix,
      70.f * (M_PI / 180.f),
      view_width / view_height,
      0.1f,
      50.f);
  srview_set_view(view, gCameraState->mtxView);
  srview_set_proj(view, gProjMatrix);

  // Perform the actual render
  GLuint view_texture;
  srview_get_output_texture(view, &view_texture);

  ImGui::Image(
      (void*)view_texture,
      content_avail,
      ImVec2(0.0f, 1.0f),
      ImVec2(1.0f, 0.0f));

  ImGuiIO& io = ImGui::GetIO();

  ShowViewManip();
  ShowTransformManip(
      (float*)&(gCameraState->mtxView),
      (float*)&(gProjMatrix),
      (float*)(&gTransformMatrix));

  ImGui::End();
};

int main() {
  gTimerInit();

  LoggingInit();

  if (signal(SIGUSR1, signal_handler) == SIG_ERR) {
    gLog("Error registering signal handler!");
  }

  // Initialize GLFW
  glfwSetErrorCallback(error_callback);
  glfwInit();

  const char* glsl_version = "#version 130";
  glfwWindowHint(GLFW_CLIENT_API, GLFW_OPENGL_API);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
  glfwWindowHint(GLFW_SAMPLES, 16);
  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

#if __APPLE__
  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
#endif

  gWindow = glfwCreateWindow(800, 600, "ViSimir", NULL, NULL);
  assert(gWindow != NULL);
  glfwMakeContextCurrent(gWindow);
  glfwSwapInterval(1);

  // Initialize OpenGL loader
  gladLoadGL(glfwGetProcAddress);

  glfwSetKeyCallback(gWindow, key_callback);
  glfwSetScrollCallback(gWindow, mouse_scroll_callback);

  std::cout << "OpenGL Version: " << glGetString(GL_VERSION) << endl;
  std::cout << "GLSL Version  : " << glGetString(GL_SHADING_LANGUAGE_VERSION)
            << endl;

  glEnable(GL_DEBUG_OUTPUT);
  //  glEnable(GL_DEBUG_OUTPUT_SYNCHRONOUS);
  glDebugMessageCallback(opengl_error_callback, nullptr);

  // During init, enable debug output
  //	glEnable              ( GL_DEBUG_OUTPUT );
  //	glDebugMessageCallback( (GLDEBUGPROC) opengl_error_callback, 0 );

  // imgui initialization.
  IMGUI_CHECKVERSION();
  ImGui::CreateContext();
  ImGuiIO& io = ImGui::GetIO();
  (void)io;
  io.ConfigFlags |= ImGuiConfigFlags_DockingEnable;
  //	io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable;

  GuiInputState gui_input_state;
  gGuiInputState = &gui_input_state;

  ImGui_ImplGlfw_InitForOpenGL(gWindow, true);
  ImGui_ImplOpenGL3_Init(glsl_version);

  // Setup Style
  ImGui::StyleColorsDark();
  //ImGui::StyleColorsClassic();

  ImGuiStyle& style = ImGui::GetStyle();
  style.WindowRounding =
      0.0f;  // When viewports are enabled it is preferable to disable WinodwRounding
  style.Colors[ImGuiCol_WindowBg].w =
      1.0f;  // When viewports are enabled it is preferable to disable WindowBg alpha

  double frame_time_last = glfwGetTime();
  double frame_time_current = frame_time_last;
  double frame_delta_time = 0.0;
  uint64_t frame_counter = 0;

  bool show_demo_window = true;
  ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f);

  CameraState camera_state;
  CameraState_Init(&camera_state);
  gCameraState = &camera_state;

  Dialogs dialogs;
  gDialogs = &dialogs;

  gRndr = srndr_create();
  gView = srview_create();
  gRndrCmds = srcmdbuf_create(1024);

  //
  // Scene init
  //
  simulator_init();
  gHullScene.init();

  simd4x4f_translation(&gCameraState->mtxView, 0.1f, 0.1f, -0.5f);
  simd4f eye = simd4f_create(5.f, 4.f, 5.f, 1.f);

  simd4x4f_lookat(
      &gCameraState->mtxView,
      eye,
      simd4f_create(0.f, 0.f, 0.f, 1.f),
      simd4f_create(0.f, 1.f, 0.f, 1.f));

  simd4x4f scale_mat = simd4x4f_scale(simd4f_create(1.f, 1.f, 1.f, 1.f));
  simd4x4f_mul(&gTransformMatrix, &scale_mat, &gTransformMatrix);

  CameraState_CalcFromMatrix(gCameraState, &gCameraState->mtxView);

  while (!glfwWindowShouldClose(gWindow)) {
    FrameMark;
    frame_counter++;

    {
      ZoneNamedN(input_loop, "InputEvents", true);
      // Start the imgui frame such that widgets can be submitted
      handle_mouse();

      glfwPollEvents();
    }

    {
      ZoneNamedN(imgui_render, "ImGuiFrameStart", true);
      ImGui_ImplOpenGL3_NewFrame();
      ImGui_ImplGlfw_NewFrame();
      ImGui::NewFrame();
      ShowDockspace(true);
      ImGuizmo::BeginFrame();
      ImGuizmo::Enable(gGizmoEnabled);
    }

    frame_time_last = frame_time_current;
    frame_time_current = glfwGetTime();
    frame_delta_time = frame_time_current - frame_time_last;

    gTimer->mFrameTime = (float)(frame_delta_time);
    if (!gTimer->mPaused) {
      gTimer->mDeltaTime = gTimer->mFrameTime;
      gTimer->mCurrentTime = gTimer->mCurrentTime + gTimer->mDeltaTime;
    } else {
      gTimer->mDeltaTime = 0.0f;
    }

    {
      ZoneNamedN(main_ui, "UpdateMainUI", true);
      assert(gTimer->mDeltaTime >= 0.0f);
      int width, height;
      ImGui::BeginMainMenuBar();

      if (ImGui::BeginMenu("Dialogs")) {
        ImGui::Checkbox("Camera", &gDialogs->mCameraDialog);
        ImGui::Checkbox("Simulator", &gDialogs->mSimulator);
        ImGui::Checkbox("ImGuizmo Transform", &gDialogs->mImGuizmoTransform);
        ImGui::Checkbox("ImGui Demo", &gDialogs->mImGuiDemo);
        ImGui::EndMenu();
      }

      float menu_bar_height = ImGui::GetWindowHeight();
      ImGui::EndMainMenuBar();

      if (glfwGetMouseButton(gWindow, GLFW_MOUSE_BUTTON_RIGHT)) {
        if (gControlMode == ControlModeNone) {
          gControlMode = ControlModeFirstPersonView;
          CameraState_CalcFromMatrix(gCameraState, &gCameraState->mtxView);
          glfwSetInputMode(gWindow, GLFW_CURSOR, GLFW_CURSOR_DISABLED);
        }
      } else {
        gControlMode = ControlModeNone;
        glfwSetInputMode(gWindow, GLFW_CURSOR, GLFW_CURSOR_NORMAL);
      }

      if (gControlMode == ControlModeFirstPersonView) {
        float camera_accel[3] = {0.f, 0.f, 0.f};
        float accel_scale = 100.0;

        if (glfwGetKey(gWindow, GLFW_KEY_LEFT_SHIFT)) {
          accel_scale *= 3.;
        } else if (glfwGetKey(gWindow, GLFW_KEY_LEFT_CONTROL)) {
          accel_scale /= 3.;
        }

        if (glfwGetKey(gWindow, GLFW_KEY_W)) {
          camera_accel[0] -= accel_scale;
        }
        if (glfwGetKey(gWindow, GLFW_KEY_S)) {
          camera_accel[0] += accel_scale;
        }
        if (glfwGetKey(gWindow, GLFW_KEY_C)) {
          camera_accel[1] -= accel_scale;
        }
        if (glfwGetKey(gWindow, GLFW_KEY_SPACE)) {
          camera_accel[1] += accel_scale;
        }
        if (glfwGetKey(gWindow, GLFW_KEY_A)) {
          camera_accel[2] -= accel_scale;
        }
        if (glfwGetKey(gWindow, GLFW_KEY_D)) {
          camera_accel[2] += accel_scale;
        }

        CameraState_Update(
            gCameraState,
            frame_delta_time,
            gGuiInputState->mousedX,
            gGuiInputState->mousedY,
            camera_accel);
      }

      if (gDialogs->mCameraDialog) {
        ImGui::Begin("Camera Controls", &gDialogs->mCameraDialog);

        bool changed = false;
        changed = ImGui::SliderFloat(
            "Heading",
            &gCameraState->heading,
            -M_PI * 0.98,
            M_PI * 0.98);
        changed = ImGui::SliderFloat(
                      "Pitch",
                      &gCameraState->pitch,
                      -M_PI * 0.49,
                      M_PI * 0.49)
                  || changed;

        changed =
            ImGui::SliderFloat3("Forward", gCameraState->forward, -1.f, 1.f)
            || changed;
        changed = ImGui::SliderFloat3("Right", gCameraState->right, -1.f, 1.f)
                  || changed;
        changed =
            ImGui::SliderFloat3("Up", gCameraState->up, -1.f, 1.f) || changed;
        changed = ImGui::SliderFloat3("Pos", gCameraState->pos, -100.f, 100.f)
                  || changed;

        if (changed) {
          CameraState_CalcToMatrix(gCameraState, &gCameraState->mtxView);
        }

        if (gDialogs->mImGuiDemo) {
          ImGui::ShowDemoWindow(&gDialogs->mImGuiDemo);
        }

        ImGui::End();
      }

      //gHullScene.draw(gRndrCmds);
      SimulatorSceneRender(gRndrCmds);
      srndr_render(gRndr, gView, gRndrCmds);
      ImSRenderWidget(gRndr, gView);
    }

    simulator_update(gTimer->mDeltaTime);

    {
      ZoneNamedN(render_submit, "RenderSubmit", true);
      glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);

      {
        ZoneNamedN(imgui_render, "ImGuiRender", true);
        // Rendering
        ImGui::Render();
        int display_w, display_h;
        glfwMakeContextCurrent(gWindow);
        glfwGetFramebufferSize(gWindow, &display_w, &display_h);
        glViewport(0, 0, display_w, display_h);
        glClearColor(
            clear_color.x,
            clear_color.y,
            clear_color.z,
            clear_color.w);
        glClear(GL_COLOR_BUFFER_BIT);
        ImDrawData* draw_data = ImGui::GetDrawData();
        assert(draw_data != NULL);
        ImGui_ImplOpenGL3_RenderDrawData(draw_data);
      }

      // Update and Render additional Platform Windows
      if (io.ConfigFlags & ImGuiConfigFlags_ViewportsEnable) {
        ImGui::UpdatePlatformWindows();
        ImGui::RenderPlatformWindowsDefault();
      }

      {
        ZoneNamedN(gl_swap, "Swap", true);
        glfwMakeContextCurrent(gWindow);
        glfwSwapBuffers(gWindow);
      }

      {
        ZoneNamedN(fps_limit, "FPS limit", true);
        // Send the application to sleep if we have some time left for this frame
        double frame_target_time = 1.0 / 60.0;
        if (frame_delta_time < frame_target_time) {
          usleep((frame_target_time - frame_delta_time) * 1000000 * 0.98);
        }
      }
    }
  }

  gLog("Exiting application");

  gHullScene.destroy();

  srview_destroy(gView);
  srndr_destroy(gRndr);

  ImGui_ImplOpenGL3_Shutdown();
  ImGui_ImplGlfw_Shutdown();
  ImGui::DestroyContext();
  glfwTerminate();
}