/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"	 // Precompiled header
//
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/opengl/CArrow.h>
#include <mrpt/opengl/CBox.h>
#include <mrpt/opengl/CCylinder.h>
#include <mrpt/opengl/CPolyhedron.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CSetOfTriangles.h>
#include <mrpt/opengl/CSimpleLine.h>
#include <mrpt/opengl/stock_objects.h>

using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

/*---------------------------------------------------------------
					RobotPioneer
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotPioneer()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	ret->setName("theRobot");

	CSetOfTriangles::Ptr obj = std::make_shared<CSetOfTriangles>();
	obj->setLocalRepresentativePoint({0.0f, 0.0f, 0.15f});

	// Add triangles:
	mrpt::opengl::TTriangle t;

	t.r(0) = t.r(1) = t.r(2) = 255;
	t.g(0) = t.g(1) = t.g(2) = 0;
	t.b(0) = t.b(1) = t.b(2) = 0;
	t.a(0) = t.a(1) = t.a(2) = 255;

	t.vertex(0) = {0.10f, -0.10f, 0.20f};
	t.vertex(1) = {-0.20f, 0.10f, 0.25f};
	t.vertex(2) = {-0.20f, -0.10f, 0.25f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 0
	t.vertex(0) = {0.10f, -0.10f, 0.20f};
	t.vertex(1) = {0.10f, 0.10f, 0.20f};
	t.vertex(2) = {-0.20f, 0.10f, 0.25f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 1

	t.vertex(0) = {0.10f, -0.10f, 0.05f};
	t.vertex(1) = {0.10f, 0.10f, 0.20f};
	t.vertex(2) = {0.10f, -0.10f, 0.20f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 2
	t.vertex(0) = {0.10f, -0.10f, 0.05f};
	t.vertex(1) = {0.10f, 0.10f, 0.05f};
	t.vertex(2) = {0.10f, 0.10f, 0.20f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 3

	t.vertex(0) = {-0.20f, -0.10f, 0.05f};
	t.vertex(1) = {-0.20f, -0.10f, 0.25f};
	t.vertex(2) = {-0.20f, 0.10f, 0.25f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 2b
	t.vertex(0) = {-0.20f, -0.10f, 0.05f};
	t.vertex(1) = {-0.20f, 0.10f, 0.25f};
	t.vertex(2) = {-0.20f, 0.10f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 3b

	t.vertex(0) = {0.10f, -0.10f, 0.20f};
	t.vertex(1) = {-0.20f, -0.10f, 0.25f};
	t.vertex(2) = {-0.20f, -0.10f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 4

	t.vertex(0) = {0.10f, -0.10f, 0.20f};
	t.vertex(1) = {-0.20f, -0.10f, 0.05f};
	t.vertex(2) = {0.10f, -0.10f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 5

	t.vertex(0) = {0.10f, 0.10f, 0.20f};
	t.vertex(1) = {-0.20f, 0.10f, 0.05f};
	t.vertex(2) = {-0.20f, 0.10f, 0.25f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 6

	t.vertex(0) = {0.10f, 0.10f, 0.20f};
	t.vertex(1) = {0.10f, 0.10f, 0.05f};
	t.vertex(2) = {-0.20f, 0.10f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 7

	t.setColor(mrpt::img::TColorf(0.05f, 0.05f, 0.05f, 1));

	t.vertex(0) = {0.00f, 0.11f, 0.00f};
	t.vertex(1) = {0.00f, 0.11f, 0.10f};
	t.vertex(2) = {0.05f, 0.11f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 8
	t.vertex(0) = {0.00f, 0.11f, 0.00f};
	t.vertex(1) = {0.00f, 0.11f, 0.10f};
	t.vertex(2) = {-0.05f, 0.11f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 9

	t.vertex(0) = {0.00f, -0.11f, 0.00f};
	t.vertex(1) = {0.00f, -0.11f, 0.10f};
	t.vertex(2) = {0.05f, -0.11f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 10
	t.vertex(0) = {0.00f, -0.11f, 0.00f};
	t.vertex(1) = {0.00f, -0.11f, 0.10f};
	t.vertex(2) = {-0.05f, -0.11f, 0.05f};
	t.computeNormals();
	obj->insertTriangle(t);	 // 11

	ret->insert(obj);

	return ret;
}

/*---------------------------------------------------------------
					CornerXYZ
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::CornerXYZ(float scale)
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	CArrow::Ptr obj = CArrow::Create(
		0, 0, 0, scale, 0, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);

	obj->setColor(1, 0, 0);
	ret->insert(obj);

	obj = CArrow::Create(
		0, 0, 0, 0, scale, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);
	obj->setColor(0, 1, 0);

	ret->insert(obj);

	obj = CArrow::Create(
		0, 0, 0, 0, 0, scale, 0.25f * scale, 0.02f * scale, 0.05f * scale);
	obj->setColor(0, 0, 1);

	ret->insert(obj);

	return ret;
}

/*---------------------------------------------------------------
					RobotRhodon
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotRhodon()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
	float height = 0;

	vector<TPoint2D> level1;
	level1.emplace_back(0.31, 0);
	level1.emplace_back(0.22, 0.24);
	level1.emplace_back(-0.22, 0.24);
	level1.emplace_back(-0.31, 0);
	level1.emplace_back(-0.22, -0.24);
	level1.emplace_back(0.22, -0.24);

	CPolyhedron::Ptr obj1 =
		opengl::CPolyhedron::CreateCustomPrism(level1, 0.38);
	obj1->setLocation(0, 0, height);
	height += 0.38f;
	obj1->setColor(0.6f, 0.6f, 0.6f);
	ret->insert(obj1);

	vector<TPoint2D> level2;
	level2.emplace_back(0.16, 0.21);
	level2.emplace_back(-0.16, 0.21);
	level2.emplace_back(-0.16, -0.21);
	level2.emplace_back(0.16, -0.21);

	CPolyhedron::Ptr obj2 =
		opengl::CPolyhedron::CreateCustomPrism(level2, 0.35);
	obj2->setLocation(0, 0, height);
	height += 0.35f;
	obj2->setColor(0.2f, 0.2f, 0.2f);
	ret->insert(obj2);

	vector<TPoint2D> level3;
	level3.emplace_back(-0.12, 0.12);
	level3.emplace_back(-0.16, 0.12);
	level3.emplace_back(-0.16, -0.12);
	level3.emplace_back(-0.12, -0.12);

	CPolyhedron::Ptr obj3 = opengl::CPolyhedron::CreateCustomPrism(level3, 1);
	obj3->setLocation(0, 0, height);
	// height+=1;
	obj3->setColor(0.6f, 0.6f, 0.6f);
	ret->insert(obj3);

	opengl::CCylinder::Ptr obj4 =
		std::make_shared<opengl::CCylinder>(0.05f, 0.05f, 0.4f, 20);
	obj4->setLocation(0, 0, 0.73);
	obj4->setColor(0.0f, 0.0f, 0.9f);
	ret->insert(obj4);

	opengl::CCylinder::Ptr obj5 =
		std::make_shared<opengl::CCylinder>(0.05f, 0.05f, 0.4f, 20);
	obj5->setPose(CPose3D(0.32, 0, 0.89, 0, -1, 0));
	obj5->setColor(0.0f, 0.0f, 0.9f);
	ret->insert(obj5);

	return ret;
}

/*---------------------------------------------------------------
					RobotGiraff
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotGiraff()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
	float height = 0;

	// Base
	vector<TPoint2D> level1;
	level1.emplace_back(0.31, 0);
	level1.emplace_back(0.22, 0.24);
	level1.emplace_back(-0.22, 0.24);
	level1.emplace_back(-0.31, 0);
	level1.emplace_back(-0.22, -0.24);
	level1.emplace_back(0.22, -0.24);

	CPolyhedron::Ptr obj1 =
		opengl::CPolyhedron::CreateCustomPrism(level1, 0.23);
	obj1->setLocation(0, 0, height);
	height += 0.23f;
	obj1->setColor(1.0f, 0.6f, 0.0f);
	ret->insert(obj1);

	// Electronic's cage
	vector<TPoint2D> level2;
	level2.emplace_back(0.13, 0.1);
	level2.emplace_back(-0.13, 0.1);
	level2.emplace_back(-0.13, -0.1);
	level2.emplace_back(0.13, -0.1);

	CPolyhedron::Ptr obj2 =
		opengl::CPolyhedron::CreateCustomPrism(level2, 0.45);
	obj2->setLocation(0, 0, height);
	height += 0.45f;
	obj2->setColor(1.0f, 0.6f, 0.2f);
	ret->insert(obj2);

	// Neck
	vector<TPoint2D> level3;
	level3.emplace_back(0.03, 0.03);
	level3.emplace_back(-0.03, 0.03);
	level3.emplace_back(-0.03, -0.03);
	level3.emplace_back(0.03, -0.03);

	CPolyhedron::Ptr obj3 =
		opengl::CPolyhedron::CreateCustomPrism(level3, 0.55);
	obj3->setLocation(0, 0, height);
	height += 0.55f;
	obj3->setColor(0.6f, 0.6f, 0.6f);
	ret->insert(obj3);

	// Screen
	vector<TPoint2D> level4;
	level4.emplace_back(0.03, 0.11);
	level4.emplace_back(-0.03, 0.11);
	level4.emplace_back(-0.03, -0.11);
	level4.emplace_back(0.03, -0.11);

	CPolyhedron::Ptr obj4 = opengl::CPolyhedron::CreateCustomPrism(level4, 0.4);
	obj4->setLocation(0, 0, height);
	obj4->setColor(1.0f, 0.6f, 0.0f);
	ret->insert(obj4);

	return ret;
}

CSetOfObjects::Ptr stock_objects::CornerXYZEye()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
	CPose3D rotation;

	CArrow::Ptr obj = CArrow::Create(0, 0, 0, 1.0, 0, 0, 0.25f, 0.02f, 0.05f);

	obj->setColor(1, 0, 0);

	ret->insert(obj);

	obj = CArrow::Create(0, 0, 0, 0, 1.0, 0, 0.25f, 0.02f, 0.05f);
	obj->setColor(0, 1, 0);

	ret->insert(obj);

	obj = CArrow::Create(0, 0, -1.0, 0, 0, 0, 0.25f, 0.02f, 0.05f);
	obj->setColor(0, 0, 1);

	ret->insert(obj);

	return ret;
}

/*---------------------------------------------------------------
					BumblebeeCamera
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::BumblebeeCamera()
{
	CSetOfObjects::Ptr camera = std::make_shared<opengl::CSetOfObjects>();

	CPolyhedron::Ptr rect = opengl::CPolyhedron::CreateCubicPrism(
		-0.02, 0.14, -0.02, 0.02, 0, -0.04);
	rect->setColor(1.0f, 0.8f, 0.0f);

	camera->insert(rect);

	CCylinder::Ptr lCam =
		std::make_shared<opengl::CCylinder>(0.01f, 0.01f, 0.003f, 10);
	lCam->setColor(1, 0, 0);

	CCylinder::Ptr rCam =
		std::make_shared<opengl::CCylinder>(0.01f, 0.01f, 0.003f, 10);
	rCam->setPose(CPose3D(0.12, 0, 0));
	rCam->setColor(0, 0, 0);

	camera->insert(lCam);
	camera->insert(rCam);

	return camera;
}

CSetOfObjects::Ptr stock_objects::CornerXYZSimple(float scale, float lineWidth)
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	// Using OpenGL shaders, it's more complicated to set line widths.
	// So, let's use cylinders of a diameter proportional to "scale":
	const float R = scale * 0.01f;
	const int nSlices = 6;

	{  // X:
		auto lin = CCylinder::Create(R, R, scale, nSlices);
		lin->setColor(1, 0, 0);
		lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 90 0]"));
		ret->insert(lin);
	}
	{  // Y:
		auto lin = CCylinder::Create(R, R, scale, nSlices);
		lin->setColor(0, 1, 0);
		lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 0 -90]"));
		ret->insert(lin);
	}
	{  // Z:
		auto lin = CCylinder::Create(R, R, scale, nSlices);
		lin->setColor(0, 0, 1);
		ret->insert(lin);
	}
	return ret;
}

CSetOfObjects::Ptr stock_objects::CornerXYSimple(float scale, float lineWidth)
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	// Using OpenGL shaders, it's more complicated to set line widths.
	// So, let's use cylinders of a diameter proportional to "scale":
	const float R = scale * 0.01f;
	const int nSlices = 6;

	{  // X:
		auto lin = CCylinder::Create(R, R, scale, nSlices);
		lin->setColor(1, 0, 0);
		lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 90 0]"));
		ret->insert(lin);
	}
	{  // Y:
		auto lin = CCylinder::Create(R, R, scale, nSlices);
		lin->setColor(0, 1, 0);
		lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 0 -90]"));
		ret->insert(lin);
	}
	return ret;
}

CSetOfObjects::Ptr stock_objects::Hokuyo_URG()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	{
		CBox::Ptr base = std::make_shared<CBox>(
			TPoint3D(-0.025, -0.025, -0.0575), TPoint3D(0.025, 0.025, -0.0185));
		base->setColor(0.7f, 0.7f, 0.7f);
		ret->insert(base);
	}
	{
		CCylinder::Ptr cyl1 = std::make_shared<CCylinder>(0.02f, 0.02f, 0.01f);
		cyl1->setColor(0, 0, 0);
		cyl1->setLocation(0, 0, -0.014);
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			std::make_shared<CCylinder>(0.02f, 0.0175f, 0.01f);
		cyl2->setColor(0, 0, 0);
		cyl2->setLocation(0, 0, -0.004);
		ret->insert(cyl2);
	}
	{
		CCylinder::Ptr cyl3 =
			std::make_shared<CCylinder>(0.0175f, 0.0175f, 0.01f);
		cyl3->setColor(0, 0, 0);
		cyl3->setLocation(0, 0, 0.004);
		ret->insert(cyl3);
	}

	return ret;
}

CSetOfObjects::Ptr stock_objects::Hokuyo_UTM()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	{
		CBox::Ptr base = std::make_shared<CBox>(
			TPoint3D(-0.03, -0.03, -0.055), TPoint3D(0.03, 0.03, -0.014));
		base->setColor(0, 0, 0);
		ret->insert(base);
	}
	{
		CCylinder::Ptr cyl1 =
			std::make_shared<CCylinder>(0.028f, 0.024f, 0.028f);
		cyl1->setColor(0, 0, 0);
		cyl1->setPose(CPose3D(0, 0, -0.014));
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			std::make_shared<CCylinder>(0.028f, 0.028f, 0.01f);
		cyl2->setColor(1.0f, 69.0f / 255.0f, 0);
		cyl2->setLocation(0, 0, 0.014);
		ret->insert(cyl2);
	}
	{
		CCylinder::Ptr cyl3 =
			std::make_shared<CCylinder>(0.028f, 0.028f, 0.01f);
		cyl3->setColor(0, 0, 0);
		cyl3->setLocation(0, 0, 0.024);
		ret->insert(cyl3);
	}

	return ret;
}

CSetOfObjects::Ptr stock_objects::Househam_Sprayer()
{
	CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

	{
		CBox::Ptr cabin = std::make_shared<CBox>(
			TPoint3D(0.878, 0.723, -0.12), TPoint3D(-0.258, -0.723, -1.690));
		cabin->setColor(0.7f, 0.7f, 0.7f);
		ret->insert(cabin);
	}
	{
		CBox::Ptr back = std::make_shared<CBox>(
			TPoint3D(-0.258, 0.723, -0.72), TPoint3D(-5.938, -0.723, -1.690));
		back->setColor(1, 1, 1);
		ret->insert(back);
	}
	{
		CBox::Ptr boomAxis = std::make_shared<CBox>(
			TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, -0.723, -1.690));
		boomAxis->setColor(0, 0, 0);
		ret->insert(boomAxis);
	}
	{
		CBox::Ptr boom1 = std::make_shared<CBox>(
			TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, 11.277, -1.620));
		boom1->setColor(0, 1, 0);
		ret->insert(boom1);
	}
	{
		CBox::Ptr boom2 = std::make_shared<CBox>(
			TPoint3D(-5.938, -0.723, -1.0), TPoint3D(-6.189, -11.277, -1.620));
		boom2->setColor(0, 1, 0);
		ret->insert(boom2);
	}
	{
		CCylinder::Ptr cyl1 =
			std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl1->setColor(0, 0, 0);
		cyl1->setPose(CPose3D(-0.710, 0.923, -2.480, 0, 0, 90.0_deg));
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl2->setColor(0, 0, 0);
		cyl2->setPose(CPose3D(-3.937, 0.923, -2.480, 0, 0, 90.0_deg));
		ret->insert(cyl2);
	}
	{
		CCylinder::Ptr cyl1 =
			std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl1->setColor(0, 0, 0);
		cyl1->setPose(CPose3D(-0.710, -0.423, -2.480, 0, 0, 90.0_deg));
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl2->setColor(0, 0, 0);
		cyl2->setPose(CPose3D(-3.937, -0.423, -2.480, 0, 0, 90.0_deg));
		ret->insert(cyl2);
	}
	return ret;
}
