#include <iostream>
#include <opengl-playground/sdlmainwindow.hpp>
#include <opengl-playground/openglshader.hpp>
#include <opengl-playground/textured-rectangle.hpp>
#include <opengl-playground/globalcanvassettings.hpp>

//#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <opengl-playground/model.hpp>
#include <opengl-playground/imu-serial.hpp>

#include <imgui.h>
#include <imgui_impl_sdl.h>
#include <imgui_impl_opengl3.h>

#define WINDOW_WIDTH 1500
#define WINDOW_HEIGHT 1000

int main(int argc, char **argv)
{
	(void)argc;
	(void)argv;
	SdlMainWindow *window;
	bool run = true;
	glm::mat4 projection_matrix;
	float fov_angle = 45.0f;


	window = new SdlMainWindow(WINDOW_HEIGHT, WINDOW_WIDTH);

	window->show();
	window->activateGlContext();

	/* Clear the image and set the background to a nice gray */
	glViewport(0, 0, WINDOW_WIDTH, WINDOW_HEIGHT);
	glClearColor(0.1f, 0.1f, 0.1f, 0.f);
	glClear(GL_COLOR_BUFFER_BIT);

	projection_matrix = glm::ortho(-10.0f, 10.0f, -10.0f, 10.0f, -10.0f, 10.0f);
	GlobalCanvasSettings::setProjectionMatrix(projection_matrix);
	GlobalCanvasSettings::setViewMatrix(glm::mat4(1.0f));

	// Setup Dear ImGui context
	IMGUI_CHECKVERSION();
	ImGui::CreateContext();
	ImGuiIO& io = ImGui::GetIO(); (void)io;
	//io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard;     // Enable Keyboard Controls
	//io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad;      // Enable Gamepad Controls

	// Setup Dear ImGui style
	ImGui::StyleColorsDark();
	//ImGui::StyleColorsClassic();

	// Setup Platform/Renderer bindings
	ImGui_ImplSDL2_InitForOpenGL(window->getWindow(), window->getContext());
	ImGui_ImplOpenGL3_Init("#version 330");

	auto shader = std::make_shared<OpenGlShaderProgram>("shaders/mesh-vertex.glsl", "", "shaders/mesh-fragment.glsl");
	shader->compile();
	//Model imu("/tmp/imu.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader);

	Model coords("coordinate-system/coordsys.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader);

	coords.realize();

	Model imu("bmi160/bmi160.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader);

	imu.realize();

	auto imu_base_mat = glm::translate(glm::mat4(1.0f), glm::vec3(2.0,0.0,0.0));
	imu.setModelMatrix(imu_base_mat);

	glEnable(GL_DEPTH_TEST);

	auto imu_serial = ImuSerial("/dev/ttyUSB0", 115200);
	imu_serial.start();

	while (run) {

		SDL_Event event;
		while (SDL_PollEvent(&event))
		{
			ImGui_ImplSDL2_ProcessEvent(&event);
			if (event.type == SDL_QUIT)
				run = false;
			if (event.type == SDL_WINDOWEVENT && event.window.event == SDL_WINDOWEVENT_CLOSE && event.window.windowID == SDL_GetWindowID(window->getWindow()))
				run = false;

			if (event.type == SDL_MOUSEWHEEL) {
				if (event.wheel.y > 0) {
					fov_angle -= 0.1;
					if (fov_angle <= 1.0f)
						fov_angle = 1.0f;
				} else if (event.wheel.y < 0) {
					fov_angle += 0.1;
					if (fov_angle >= 60.0f)
						fov_angle = 60.0f;
				}
			}
		}


		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);


		// Start the Dear ImGui frame
		ImGui_ImplOpenGL3_NewFrame();
		ImGui_ImplSDL2_NewFrame(window->getWindow());
		ImGui::NewFrame();


		ImGui::Begin("Camera");

		static float cam_rot_y = 0.0f;
		static int e = 1;
		ImGui::RadioButton("Orthographic", &e, 0); ImGui::SameLine();
		ImGui::RadioButton("Perspective", &e, 1);

		static float view_z = 10.0f, view_x = 0.0f, view_y = 0.0f;
		ImGui::SliderFloat("Camera x", &view_x, -20.0f, 20.0f);
		ImGui::SliderFloat("Camera y", &view_y, -20.0f, 20.0f);
		ImGui::SliderFloat("Camera z", &view_z, 0.0f, 20.0f);
		ImGui::SliderFloat("Camera y rot", &cam_rot_y, -180.0, 180.0);
		ImGui::SliderFloat("Camera FOV", &fov_angle, 1.0f, 60.0f);
		ImGui::Text("Framerate: %.01f Hz", io.Framerate);


		auto camera_rotation = glm::rotate(glm::mat4(1.0f),
						   glm::radians(cam_rot_y),
						   glm::vec3(0.0f, 1.0f, 0.0f));

		auto camera_pos = camera_rotation * glm::vec4(view_x, view_y, view_z, 1.0f);
		GlobalCanvasSettings::setCameraPosition(camera_pos);
		glm::mat4 view = glm::lookAt(GlobalCanvasSettings::getCameraPosition(),
				   glm::vec3(view_x, view_y, 0.0f),
				   glm::vec3(0.0f, 1.0f, 0.0f));

		GlobalCanvasSettings::setViewMatrix(view);

		if(!e) {
			//GlobalCanvasSettings::setProjectionMatrix(glm::mat4(1.0f));
			GlobalCanvasSettings::setProjectionMatrix(glm::ortho(-5.0f, 5.0f, -5.0f, 5.0f, -5.0f, 5.0f));
		} else {

			GlobalCanvasSettings::setProjectionMatrix(glm::perspective(glm::radians(fov_angle), (float)1200/(float)800, 0.2f, 30.0f));
		}
		ImGui::End();


		coords.render();
		auto imu_rot_mat = imu_serial.get_rot_matrix();
		auto imu_correction_matrix = glm::rotate(glm::mat4(1.0f), glm::radians(90.0f), glm::vec3(1.0f, 0.0f, 0.0f));
		auto imu_correction_inverse = glm::inverse(imu_correction_matrix);

		auto imu_model_matrix = imu_base_mat * imu_correction_inverse * imu_rot_mat * imu_correction_matrix;

		imu.setModelMatrix(imu_model_matrix);
		imu.render();

		{
			ImGui::Begin("IMU Attitude & Heading");
			auto quat = imu_serial.get_quaternion();
			auto euler = glm::eulerAngles(quat) * 180.0f / (float)M_PI;
			ImGui::Text("Euler XYZ: %.0f | %.0f | %.0f", euler.x, euler.y, euler.z);
			ImGui::Text("Quat WIJK: %.02f | %.02f | %.02f | %.02f", quat.w, quat.x, quat.y, quat.z);
			ImGui::End();
		}

		{
			auto accel = imu_serial.get_accel();
			auto gyro = imu_serial.get_gyro();
			auto current_rot_matrix = imu_serial.get_rot_matrix();
			auto global_accel = current_rot_matrix * glm::vec4(accel, 0.0f);
			auto global_gyr = current_rot_matrix * glm::vec4(gyro, 0.0f);

			ImGui::Begin("IMU Data");
			ImGui::Text("GYR X: %8.2f\t\t °/s", glm::degrees(gyro.x));
			ImGui::Text("GYR Y: %8.2f\t\t °/s", glm::degrees(gyro.y));
			ImGui::Text("GYR Z: %8.2f\t\t °/s", glm::degrees(gyro.z));
			ImGui::Spacing();
			ImGui::Text("GYR right: %8.2f\t\t °/s", glm::degrees(global_gyr.x));
			ImGui::Text("GYR front: %8.2f\t\t °/s", glm::degrees(global_gyr.y));
			ImGui::Text("GYR up: %8.2f\t\t °/s", glm::degrees(global_gyr.z));
			ImGui::Separator();
			ImGui::Text("ACC X: %8.3f\t\t g", accel.x);
			ImGui::Text("ACC Y: %8.3f\t\t g", accel.y);
			ImGui::Text("ACC Z: %8.3f\t\t g", accel.z);
			ImGui::Spacing();
			ImGui::Text("ACC right\t: %8.3f\t g", global_accel.x);
			ImGui::Text("ACC front\t: %8.3f\t g", global_accel.y);
			ImGui::Text("ACC up   \t: %8.3f\t g", global_accel.z);

			ImGui::End();
		}

		ImGui::Render();
		ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());

		window->swapBuffer();

	}

	ImGui_ImplOpenGL3_Shutdown();
	ImGui_ImplSDL2_Shutdown();
	ImGui::DestroyContext();

	shader->unload();

	delete window;
}