// This example is heavily based on the tutorial at https://open.gl

// OpenGL Helpers to reduce the clutter
#include "Helpers.h"
#include "Mesh.h"
#include "Utils.h"
#include "World.h"

#ifdef __APPLE__
#define GL_SILENCE_DEPRECATION
// GLFW is necessary to handle the OpenGL context
#include <GLFW/glfw3.h>
#else
// GLFW is necessary to handle the OpenGL context
#include <GLFW/glfw3.h>
#endif

// Linear Algebra Library
#include <Eigen/Core>

// Timer
#include <chrono>
#include <iostream>

World world;

void framebuffer_size_callback(GLFWwindow* window, int width, int height) {
    glViewport(0, 0, width, height);
}

Vector3f rayOrigin(0.0, 0.0, 0.0);
Vector3f rayDirection(0.0, 0.0, 0.0);

//vector<Mesh> meshes;
Mesh meshes[100] = {Mesh()};
int meshArrayTop = 0;

bool isCameraMovingInCartesianCoords = false;

Vector3f screenCoordsToWorldCoords(GLFWwindow* window, Vector3d screenCoords) {
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    double xpos = screenCoords(0);
    double ypos = screenCoords(1);
    double zpos = screenCoords(2);

    // Convert mouse position to world coordinates
    Vector4f p_screen(xpos,height-1-ypos,zpos,1);
    Vector4f p_canonical((p_screen[0]/width)*2-1,((p_screen[1]/height)*2)-1, zpos, 1);
    Vector4f p_camera = world.getViewCamera().getProjection().inverse() * p_canonical;
    Vector4f p_world = world.getViewCamera().getView().inverse() * p_camera;

    return Vector3f(p_world(0), p_world(1), p_world(2));
}

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods) {

    // Update the position of the first vertex if the left button is pressed
    if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS){
        double xpos, ypos;
        glfwGetCursorPos(window, &xpos, &ypos);
        Vector3f worldPoint = screenCoordsToWorldCoords(window, Vector3d(xpos, ypos, 0.0));
        Vector3f worldPoint2;
        if (world.getViewCamera().getProjectionType() == Camera::PROJECTION_PERSPECTIVE) {
            worldPoint2 = world.getViewCamera().getCameraPosition();
        } else {
            worldPoint2 = screenCoordsToWorldCoords(window, Vector3d(xpos, ypos, 1.0));
        }

        rayOrigin = worldPoint;
        rayDirection = (worldPoint - worldPoint2).normalized();

        int closestMeshIntersectedIndex = -1;
        float closestMeshDistance = 999999.0;
        for (int meshNo = 0; meshNo < world.getMeshes().size(); meshNo++) {
            Mesh mesh = world.getMeshes().at(meshNo).get();
            MatrixXf model = mesh.getModel();
            MatrixXf triangles = mesh.getTriangleVertices();
            for (long i = 0; i < triangles.cols(); i += 3) {
                Vector4f a4 = model * Vector4f(triangles.col(i)(0), triangles.col(i)(1), triangles.col(i)(2), 1.0);
                Vector4f b4 = model * Vector4f(triangles.col(i+1)(0), triangles.col(i+1)(1), triangles.col(i+1)(2), 1.0);
                Vector4f c4 = model * Vector4f(triangles.col(i+2)(0), triangles.col(i+2)(1), triangles.col(i+2)(2), 1.0);

                Vector3f a = Vector3f(a4(0), a4(1), a4(2));
                Vector3f b = Vector3f(b4(0), b4(1), b4(2));
                Vector3f c = Vector3f(c4(0), c4(1), c4(2));
                float t = -1;
                if (Utils::rayTriangleIntersect(rayOrigin, rayDirection, a, b, c, t)) {
                    if (t < closestMeshDistance) {
                        closestMeshDistance = t;
                        closestMeshIntersectedIndex = meshNo;
                    }
                }
            }
        }
        world.setSelectedMeshIndex(closestMeshIntersectedIndex);
    }
}

void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods) {
    Camera& camera = world.getViewCamera();
    switch (key) {
        case GLFW_KEY_1:
            if (action == GLFW_PRESS) {
                meshes[meshArrayTop] = Mesh::fromOffFile("../data/unit_cube.off", Vector3f(1.0, 1.0, 0.0), FLAT_SHADE);
                meshes[meshArrayTop].scaleToUnitCube();
                meshes[meshArrayTop].translate(Vector3f(0.0, 0.0, 0.0));
                world.addMesh(meshes[meshArrayTop]);
                meshArrayTop++;
            }
            break;
        case GLFW_KEY_2:
            if (action == GLFW_PRESS) {
                meshes[meshArrayTop] = Mesh::fromOffFile("../data/bunny.off", Vector3f(0.0, 1.0, 0.0), FLAT_SHADE);
                meshes[meshArrayTop].scaleToUnitCube();
                meshes[meshArrayTop].translate(Vector3f(-0.072, -0.1, 0.0));
                world.addMesh(meshes[meshArrayTop]);
                meshArrayTop++;
            }
            break;
        case GLFW_KEY_3:
            if (action == GLFW_PRESS) {
                meshes[meshArrayTop] = Mesh::fromOffFile("../data/bumpy_cube.off", Vector3f(1.0, 0.0, 0.0), FLAT_SHADE);
                meshes[meshArrayTop].scaleToUnitCube();
                meshes[meshArrayTop].translate(Vector3f(0.0, 0.0, 0.0));
                world.addMesh(meshes[meshArrayTop]);
                meshArrayTop++;
            }
            break;

        case GLFW_KEY_LEFT_ALT:
            if (action == GLFW_PRESS) {
                isCameraMovingInCartesianCoords = !isCameraMovingInCartesianCoords;
                cout << "Alt pressed: " << isCameraMovingInCartesianCoords << endl;
            }
            break;
        case GLFW_KEY_UP:
            if (isCameraMovingInCartesianCoords) {
                camera.translateBy(Vector3f(0.0, 0.5, 0.));
            } else {
                camera.translateByAngleOnXAxis(0.1);
            }
            break;
        case GLFW_KEY_DOWN:
            if (isCameraMovingInCartesianCoords) {
                camera.translateBy(Vector3f(0.0, -0.5, 0.));
            } else {
                camera.translateByAngleOnXAxis(-0.1);
            }
            break;
        case GLFW_KEY_LEFT:
            if (isCameraMovingInCartesianCoords) {
                camera.translateBy(Vector3f(-0.5, 0.0, 0.0));
            } else {
                camera.translateByAngleOnYAxis(-0.1);
            }
            break;
        case GLFW_KEY_RIGHT:
            if (isCameraMovingInCartesianCoords) {
                camera.translateBy(Vector3f(0.5, 0.0, 0.0));
            } else {
                camera.translateByAngleOnYAxis(0.1);
            }
            break;
        case GLFW_KEY_RIGHT_BRACKET:
            camera.translateBy(Vector3f(0.0, 0.0, -0.1));
            break;
        case GLFW_KEY_SLASH:
            camera.translateBy(Vector3f(0.0, 0.0, 0.1));
            break;

        case GLFW_KEY_TAB:
            if (action == GLFW_PRESS) {
                if (world.getViewCamera().getProjectionType() == Camera::PROJECTION_ORTHOGRAPHIC) {
                    world.getViewCamera().setProjectionType(Camera::PROJECTION_PERSPECTIVE);
                    world.getViewCamera().setFieldOfViewAngle((3.14/180) * 90);
                } else {
                    world.getViewCamera().setProjectionType(Camera::PROJECTION_ORTHOGRAPHIC);
                    world.getViewCamera().setFieldOfViewAngle((3.14/180) * 140);
                }
            }
            break;
        case GLFW_KEY_LEFT_SHIFT:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                Mesh& mesh = world.getMeshes().at(world.getSelectedMeshIndex()).get();
                if (mesh.getRenderType() == PHONG_SHADE) mesh.setRenderType(WIREFRAME);
                else if (mesh.getRenderType() == WIREFRAME) mesh.setRenderType(FLAT_SHADE);
                else if (mesh.getRenderType() == FLAT_SHADE) mesh.setRenderType(PHONG_SHADE);
            }
            break;
        case  GLFW_KEY_A:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().translate(Vector3f(-0.1, 0, 0.0));
            }
            break;
        case  GLFW_KEY_D:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().translate(Vector3f(0.1, 0, 0.0));
            }
            break;
        case  GLFW_KEY_W:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().translate(Vector3f(0.0, 0.0, -0.1));
            }
            break;
        case  GLFW_KEY_S:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().translate(Vector3f(0.0, 0, 0.1));
            }
            break;
        case  GLFW_KEY_Q:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().translate(Vector3f(0.0, 0.1, 0.0));
            }
            break;
        case  GLFW_KEY_Z:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().translate(Vector3f(0.0, -0.1, 0.0));
            }
            break;
        case  GLFW_KEY_E:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().rotate(Utils::AXIS_Z, -0.1);
            }
            break;
        case  GLFW_KEY_R:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().rotate(Utils::AXIS_Z, 0.1);
            }
            break;
        case  GLFW_KEY_F:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().rotate(Utils::AXIS_X, -0.1);
            }
            break;
        case  GLFW_KEY_G:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().rotate(Utils::AXIS_X, 0.1);
            }
            break;
        case  GLFW_KEY_C:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().rotate(Utils::AXIS_Y, -0.1);
            }
            break;
        case  GLFW_KEY_V:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().rotate(Utils::AXIS_Y, 0.1);
            }
            break;
        case  GLFW_KEY_P:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().scale(1.1);
            }
            break;
        case  GLFW_KEY_L:
            if (action == GLFW_PRESS) {
                if (world.getSelectedMeshIndex() == -1) return;
                world.getMeshes().at(world.getSelectedMeshIndex()).get().scale(1/1.1);
            }
            break;
    }
}

void window_size_callback(GLFWwindow* window, int width, int height) {
    world.getViewCamera().setAspectRatio((float)width / (float)height);
}

GLenum getPolygonDrawType(RenderType renderType) {
    switch (renderType) {
        case WIREFRAME:
            return GL_LINE;
        case FLAT_SHADE:
            return GL_FILL;
        case PHONG_SHADE:
            return GL_FILL;
        default:
            throw runtime_error("Unsupported type");
    }
}

int main()
{
    GLFWwindow* window = HelperGL::initAndCreateGLFWWindow();

    // Initialize the VAO
    // A Vertex Array Object (or VAO) is an object that describes how the vertex
    // attributes are stored in a Vertex Buffer Object (or VBO). This means that
    // the VAO is not the actual object storing the vertex data,
    // but the descriptor of the vertex data.
    VertexArrayObject VAO;
    VAO.init();
    VAO.bind();

    // Initialize the VBO with the vertices data
    // A VBO is a data container that lives in the GPU memory
    VertexBufferObject VBO_Positions;
    VBO_Positions.init();
    VBO_Positions.update(Eigen::MatrixXf(3, 0));

    VertexBufferObject VBO_VertexNormals;
    VBO_VertexNormals.init();
    VBO_VertexNormals.update(Eigen::MatrixXf(3, 0));

    VertexBufferObject VBO_FaceNormals;
    VBO_FaceNormals.init();
    VBO_FaceNormals.update(Eigen::MatrixXf(3, 0));

    // Initialize the OpenGL Program
    // A program controls the OpenGL pipeline and it must contains
    // at least a vertex shader and a fragment shader to be valid
    Program program;
    const GLchar* vertex_shader =
            "#version 150 core\n"
            "in vec3 position;\n"
            "in vec3 vertex_normal;\n"
            "in vec3 face_normal;\n"

            "uniform vec3 color;\n"
            "uniform bool flat_normal;\n"

            "uniform mat4 projection;\n"
            "uniform mat4 model;\n"
            "uniform mat4 view;\n"

            "out vec3 Normal;\n"
            "out vec3 FragPos;\n"
            "out vec3 objectColor;\n"

            "void main()"
            "{"
            "    gl_Position = projection * view * model * vec4(position, 1.0);"
            "    FragPos = vec3(model * vec4(position, 1.0f));"
            "    if(flat_normal){"
            "       Normal = mat3(transpose(inverse(model))) * face_normal;"
            "    }else{"
            "       Normal = mat3(transpose(inverse(model))) * vertex_normal;"
            "    }"
            "    objectColor = color;"
            "}";
    const GLchar* fragment_shader =
            "#version 150 core\n"

            "in vec3 Normal;"
            "in vec3 FragPos;"
            "in vec3 objectColor;"

            "out vec4 outColor;"

            "uniform vec3 lightPos;"
            "uniform vec3 viewPos;"
            "uniform bool flat_normal;"

            "void main()"
            "{"
            "    vec3 lightColor = vec3(1.0, 1.0, 1.0);"
            "      float ambientStrength = 0.01f;"
            "      vec3 ambient = ambientStrength * lightColor;"
            "      vec3 norm = normalize(Normal);"
            "      vec3 lightDir = normalize(lightPos - FragPos);"
            "      float diff = max(dot(norm, lightDir), 0.0);"
            "      vec3 diffuse = diff * lightColor;"
            "      if(flat_normal){"
            "         vec3 result = (ambient + diffuse) * objectColor;"
            "         outColor = vec4(result, 1.0);"
            "       }else{"
            "         float specularStrength = 0.5f;"
            "         vec3 viewDir = normalize(viewPos - FragPos);"
            "         vec3 reflectDir = reflect(-lightDir, norm);  "
            "         float spec = pow(max(dot(viewDir, reflectDir), 0.0), 32);"
            "         vec3 specular = specularStrength * spec * lightColor;  "
            "         vec3 result = (ambient + diffuse + specular) * objectColor;"
            "         outColor = vec4(result, 1.0);"
            "       }"
            "}";

    // Compile the two shaders and upload the binary to the GPU
    // Note that we have to explicitly specify that the output "slot" called outColor
    // is the one that we want in the fragment buffer (and thus on screen)
    program.init(vertex_shader,fragment_shader,"outColor");
    program.bind();

    // The vertex shader wants the position of the vertices as an input.
    // The following line connects the VBO we defined above with the position "slot"
    // in the vertex shader
    program.bindVertexAttribArray("position", VBO_Positions);
    program.bindVertexAttribArray("vertex_normal", VBO_VertexNormals);
    program.bindVertexAttribArray("face_normal", VBO_FaceNormals);

    // Save the current time --- it will be used to dynamically change the triangle color
    auto t_start = std::chrono::high_resolution_clock::now();

    // Register the keyboard callback
    glfwSetKeyCallback(window, key_callback);

    // Register the mouse callback
    glfwSetMouseButtonCallback(window, mouse_button_callback);

    // Update viewport
    glfwSetFramebufferSizeCallback(window, framebuffer_size_callback);

    glfwSetWindowSizeCallback(window, window_size_callback);

    int screenWidth, screenHeight;
    glfwGetWindowSize(window, &screenWidth, &screenHeight);
    Camera camera(Vector3f(0., 0., 3.), Vector3f(0., 0., 0.),
            Camera::PROJECTION_PERSPECTIVE, (float)screenWidth / (float)screenHeight, -0.5, -100.0, (3.14/180) * 90);
    world.addCamera(camera);

    // Loop until the user closes the window
    while (!glfwWindowShouldClose(window)) {
        // Bind your VAO (not necessary if you have only one)
        VAO.bind();

        // Bind your program
        program.bind();

        // Set the uniform value depending on the time difference
        auto t_now = std::chrono::high_resolution_clock::now();
        float time = std::chrono::duration_cast<std::chrono::duration<float>>(t_now - t_start).count();
        glUniform3f(program.uniform("triangleColor"), (float)(sin(time * 4.0f) + 1.0f) / 2.0f, 0.0f, 0.0f);

        // Clear the framebuffer
        glEnable(GL_DEPTH_TEST);
        glDepthFunc(GL_LESS);
        glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

        //Set the camera view
        glUniformMatrix4fv(program.uniform("view"), 1, GL_FALSE, world.getViewCamera().getView().data());
        glUniformMatrix4fv(program.uniform("projection"), 1, GL_FALSE, world.getViewCamera().getProjection().data());
        // Draw each mesh

        for (int meshIndex = 0; meshIndex < world.getMeshes().size(); meshIndex++) {
            Mesh mesh = world.getMeshes().at(meshIndex).get();
            VBO_Positions.update(mesh.getTriangleVertices());
            VBO_VertexNormals.update(mesh.getVertexNormals());
            VBO_FaceNormals.update(mesh.getFaceNormals());

            glUniformMatrix4fv(program.uniform("model"), 1, GL_FALSE, mesh.getModel().data());
            glPolygonMode(GL_FRONT_AND_BACK, getPolygonDrawType(mesh.getRenderType()));
            if (world.getSelectedMeshIndex() == meshIndex) {
                glUniform3f(program.uniform("color"), 0.0, 0.0, 1.0);
            } else {
                glUniform3f(program.uniform("color"), mesh.getColor()(0), mesh.getColor()(1), mesh.getColor()(2));
            }
            glUniform1i(program.uniform("flat_normal"), mesh.getRenderType() != PHONG_SHADE);
            glUniform3f(program.uniform("lightPos"), -5.0, 0.0, 10.0);
            glUniform3f(program.uniform("viewPos"), camera.getCameraPosition()(0), camera.getCameraPosition()(1),
                        camera.getCameraPosition()(2));

            glDrawArrays(GL_TRIANGLES, 0, mesh.getFaces().cols() * mesh.getFaces().rows());

            if (mesh.getRenderType() == FLAT_SHADE) {
                glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
                glUniform3f(program.uniform("color"), 0.0, 0.0, 0.0);
                glDrawArrays(GL_TRIANGLES, 0, mesh.getFaces().cols() * mesh.getFaces().rows());
            }
        }

//        MatrixXf identityModel = MatrixXf::Identity(4, 4);
//        glUniformMatrix4fv(program.uniform("model"), 1, GL_FALSE, identityModel.data());
//        MatrixXf line(3, 2);
//        line << rayOrigin(0), (rayOrigin + (100 * rayDirection))(0),
//                rayOrigin(1), (rayOrigin + (100 * rayDirection))(1),
//                rayOrigin(2), (rayOrigin + (100 * rayDirection))(2);
//        VBO_Positions.update(line);
//        glDrawArrays(GL_LINE_STRIP, 0, 2);

        // Swap front and back buffers
        glfwSwapBuffers(window);

        // Poll for and process events
        glfwPollEvents();
    }

    // Deallocate opengl memory
    program.free();
    VAO.free();
    VBO_Positions.free();

    // Deallocate glfw internals
    glfwTerminate();
    return 0;
}


#include <condition_variable>
#include <queue>
#include <thread>
#include <math.h>
#include <dirent.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <pcl_conversions/pcl_conversions.h>
#include <std_msgs/Bool.h>
#include <tf/transform_listener.h>

#include <Eigen/Core>

namespace {

struct tile {
	std::string path;
	float origin_x;
	float origin_y;
	int id;
	std::vector<tile*> neighbors;
};

bool checkNeighbor(tile i, tile j, float thresh)
{
	float dist = abs(i.origin_x - j.origin_x) + abs(i.origin_y - j.origin_y);
	return dist <= thresh;
}

class ReflectivityMapLoader {
private:
	ros::NodeHandle& nodeHandle_;
	ros::Publisher pcdPublisher_;
	ros::Subscriber poseSubscriber_;
	Eigen::Vector3d pose_;
	pcl::PointCloud<pcl::PointXYZRGB> cloudFull_;
	std::string pcdPublishTopic_;
	std::string poseSubscribeTopic_;
	double visibilityRadius_;
	std::vector<tile> allTiles_;
	bool poseAvailable_;
	float neighborDist_;
	float publishRate_;

public:
	ReflectivityMapLoader(ros::NodeHandle &nodeHandle);
	~ReflectivityMapLoader();
	void readParameters();
	void createPcd(const std::vector<std::string>& pcdPaths);
	void publishPcd (sensor_msgs::PointCloud2 &cloud);
	void poseCallback(const geometry_msgs::PoseStamped &msg);
	void getPoseTile();
	double computeDist(Eigen::Vector3d p, Eigen::Vector3d q);
	void createTiles(std::vector<std::string> pcdPaths);
	void getTileCenters(std::string path, float &x, float &y);
	void computeNeighbors();
	bool onTile(Eigen::Vector3d pose, tile t);
	void managePcdPublish(tile t);
	void getPublishRate(float &rate);
};

ReflectivityMapLoader::ReflectivityMapLoader(ros::NodeHandle &nodeHandle)
	:nodeHandle_(nodeHandle),
	poseAvailable_(0)
{
	readParameters();
	poseSubscriber_ = nodeHandle_.subscribe(poseSubscribeTopic_, 500, &ReflectivityMapLoader::poseCallback, this);
	pcdPublisher_ = nodeHandle_.advertise<sensor_msgs::PointCloud2>(pcdPublishTopic_, publishRate_);
}

ReflectivityMapLoader::~ReflectivityMapLoader()
{

}

void ReflectivityMapLoader::readParameters()
{
  nodeHandle_.param("pcd_topic", pcdPublishTopic_, std::string("/reflectivity_map"));
  nodeHandle_.param("pose_topic", poseSubscribeTopic_, std::string("/pose_ground_truth"));
	nodeHandle_.param("neighbor_dist", neighborDist_, (float)192.0); // factor of 64
	nodeHandle_.param("publish_rate", publishRate_, (float)1.0); // factor of 64
}

void ReflectivityMapLoader::poseCallback(const geometry_msgs::PoseStamped &msg)
{
	pose_(0) = msg.pose.position.x;
	pose_(1) = msg.pose.position.y;
	pose_(2) = msg.pose.position.z;
	poseAvailable_ = 1;
}

void ReflectivityMapLoader::createPcd(const std::vector<std::string>& pcdPaths)
{
	sensor_msgs::PointCloud2 pcdFull, pcdPart;
	for (const std::string& path : pcdPaths) {
		if (pcdFull.width == 0) {
			if (pcl::io::loadPCDFile(path.c_str(), pcdFull) == -1) {
			}
		} else {
			if (pcl::io::loadPCDFile(path.c_str(), pcdPart) == -1) {
			}
			pcdFull.width += pcdPart.width;
			pcdFull.row_step += pcdPart.row_step;
			pcdFull.data.insert(pcdFull.data.end(), pcdPart.data.begin(), pcdPart.data.end());
		}
		if (!ros::ok()) break;
	}

	publishPcd(pcdFull);
}

void ReflectivityMapLoader::getPoseTile()
{

	// find which tile is the vehicle in
	if (poseAvailable_)
	{
		Eigen::Vector3d currentPose(pose_);
		for (std::vector<tile>::iterator i = allTiles_.begin(); i != allTiles_.end(); i++)
		{
			bool isInside = onTile(currentPose, *i);
			if (isInside)
			{
				managePcdPublish(*i);
				return;
			}
		}
	}
	ROS_INFO("Waiting for pose");
	return;
}

void ReflectivityMapLoader::managePcdPublish(tile t)
{
	std::vector<std::string> toBeLoaded;
	std::vector<int> currentlyLoaded_;
	// Load the neighbors
	for (std::vector<tile*>::iterator i = t.neighbors.begin(); i != t.neighbors.end(); i++)
	{
		currentlyLoaded_.push_back((*i)->id);
		toBeLoaded.push_back((*i)->path);
	}

	ROS_INFO("Loading %d tiles",(int)toBeLoaded.size());
	createPcd(toBeLoaded);
}

void ReflectivityMapLoader::publishPcd(sensor_msgs::PointCloud2 &pcd)
{
	if (pcd.width != 0) {
		pcd.header.frame_id = "map";
		pcdPublisher_.publish(pcd);
	}
}

double ReflectivityMapLoader::computeDist(Eigen::Vector3d p, Eigen::Vector3d q)
{
	double dist = sqrt(pow(p(0) - q(0), 2) +
										 pow(p(1) - q(1), 2));
	return dist;
}

void ReflectivityMapLoader::getTileCenters(std::string path, float &x, float &y)
{
	std::string splitPath;
	std::vector<size_t> vec;
	size_t pos = path.find('/');
	while (pos != std::string::npos)
	{
		vec.push_back(pos);
		pos = path.find('/', pos + 1);
	}
	pos = vec.back();
	std::string filename = path.substr(pos + 1, path.find('.'));
	pos = filename.find('_');
	size_t pos_end = filename.find('.');
	std::string x1 = filename.substr(0,pos);
	std::string y1 = filename.substr(pos+1,pos_end - pos -1);
	x = std::stof(x1);
	y = std::stof(y1);
}

void ReflectivityMapLoader::createTiles(std::vector<std::string> pcdPaths)
{
	int id_counter = 0;
	for (std::vector<std::string>::iterator i = pcdPaths.begin(); i != pcdPaths.end(); i++)
	{
		float x,y;
		getTileCenters(*i, x, y);
		tile newTile;
		newTile.origin_x = x;
		newTile.origin_y = y;
		newTile.path = *i;
		newTile.id = id_counter;
		allTiles_.push_back(newTile);
		id_counter++;
	}
	ROS_INFO("Total number of tiles added are %d",(int)allTiles_.size());
}

void ReflectivityMapLoader::computeNeighbors()
{
	for (std::vector<tile>::iterator i = allTiles_.begin(); i != allTiles_.end(); i++)
	{
		for (std::vector<tile>::iterator j = allTiles_.begin(); j != allTiles_.end(); j++)
		{
			bool isNeighbor = checkNeighbor(*i, *j, neighborDist_);
			if (isNeighbor)
			{
				i->neighbors.push_back(&(*j));
			}

		}
	}
}

bool ReflectivityMapLoader::onTile(Eigen::Vector3d pose, tile t)
{
	float pos_x = (float)pose(0);
	float pos_y = (float)pose(1);
	return (pos_x > t.origin_x && pos_y > t.origin_y &&
					pos_x < t.origin_x + 64.0 && pos_y < t.origin_y + 64);
}

bool getFileNames(std::string folderPath, std::vector<std::string> &pcdPaths)
{
	DIR* dirp = opendir(folderPath.c_str());
	struct dirent * dp;
	if (dirp != NULL) {
		while ((dp = readdir(dirp)) != NULL) {

			// only add pcd files
			std::size_t type = std::string(dp->d_name).find(".pcd");
			if (type != std::string::npos) {
				pcdPaths.push_back(folderPath + std::string(dp->d_name));
			}
		}
		closedir(dirp);
	} else {
		/* could not open directory */
  	perror ("Cannot open dir");
  	return EXIT_FAILURE;
	}
	return EXIT_SUCCESS;
}

void ReflectivityMapLoader::getPublishRate(float &rate)
{
	rate = publishRate_;
	return;
}

void print_usage()
{
	ROS_ERROR_STREAM("Usage:");
	ROS_ERROR_STREAM("rosrun map_loader reflectivity_map_loader reflectivity_map_folder");
}

} // namespace

int main(int argc, char **argv)
{
	ros::init(argc, argv, "reflectivity_map_folder");
	ros::NodeHandle nh("~");
	if (argc < 2 || (argc == 2 && std::string(argv[1]) == "-h")) {
		print_usage();
		return EXIT_FAILURE;
	}

	std::string folderPath("");
	if (argc == 2){
    folderPath = std::string(argv[1]);
  }

	ReflectivityMapLoader reflectivityMapLoader(nh);
	std::vector<std::string> pcdPaths;

	if (!getFileNames(folderPath, pcdPaths)) {
		reflectivityMapLoader.createTiles(pcdPaths);
		reflectivityMapLoader.computeNeighbors();

		float publishRate;
		reflectivityMapLoader.getPublishRate(publishRate);
		ros::Rate rate(publishRate);
		while (ros::ok())
		{
			reflectivityMapLoader.getPoseTile();
			ros::spinOnce();
			rate.sleep();
		}
	} else {
		return EXIT_FAILURE;
	}

	return 0;
}


/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#include <boost/multi_array.hpp>
#include "BlockCoined.hpp"
#include <queue>
using namespace std;
using namespace eosio;

char rand()
{
   capi_checksum256 result;
   auto mixedBlock = tapos_block_prefix() * tapos_block_num();
   char r;
   const char *mixedChar = reinterpret_cast<const char *>(&mixedBlock);
   sha256((char *)mixedChar, sizeof(mixedChar), &result);
   const char *p64 = reinterpret_cast<const char *>(&result);

   for (int i = 0; i < 6; i++)
   {
      r = (abs((int64_t)p64[i]) % (49 + 1 - 1)) + 1;
   }
   return r;
}

string BlockCoined::ran_char(int mod_int, char &sudo_seed, int num_char, int nounce)
{
   char prev_sudo_seed = sudo_seed;
   //print( "ran_char_start\n ");
   int r;
   std::string jewels;
   time_t timer;

   char seed;
   capi_checksum256 random_num_hash;
   uint64_t p64;

   // initialize the random number generator
   //print( "curent time is: ", std::to_string( current_time() ));
   //print( "\n ");
   auto mixedBlock = tapos_block_prefix() * tapos_block_num();
   const char *mixedChar = reinterpret_cast<const char *>(&mixedBlock);
   seed = current_time() + mixedChar[0] + nounce;
   //random_num_hash = sha256( &seed, sizeof(seed) );
   sha256((char *)&seed, sizeof(seed) * 2, &random_num_hash);
   int random = sudo_seed % 32 - 1;
   //print( "curent sudo_seed is: ", std::to_string( sudo_seed ));
   //print( "\n ");
   //print( "curent hash is: ", std::to_string( random_num_hash.hash[random] ));
   //print( "\n ");
   std::string inserted_str;
   if (num_char < 32)
   {
      for (int i = 0; i < num_char; i++)
      {
         inserted_str[i + 1] = random_num_hash.hash[i] % mod_int + 'A'; // generate a random number
      }
   }
   //print( "generated is: ",   inserted_str);
   sudo_seed = sudo_seed + 1;

   return (inserted_str);
}

int BlockCoined::check_score(vector<bool> &mark_del, vector<char> &view_row)
{
   int k = 0;
   int scored = 0;
   //print( "BlockCoined::check score\n ");

   //Find and mark scored block to be deleted
   for (int i = 1; i < this->num - 1; i++)
   {
      if (view_row[i] == view_row[i - 1])
      {
         if (view_row[i] == view_row[i + 1])
         {
            scored++;
            mark_del[i] = 1;
            mark_del[i - 1] = 1;
            mark_del[i + 1] = 1;

            for (k = i + 2; k < this->num - 1; k++)
            {
               if (view_row[i] == view_row[k])
               {
                  scored++;
                  mark_del[k] = 1;
               }
               else
               {
                  break;
               }
            }
            if (scored > 1)
            {
               i = k;
            }
         }
      }
   }
   return scored;
}

void BlockCoined::delete_marked_then_fill(block_jewel &current_game, const vector<bool> mark_del, vector<char> &temp_vec, vector<uint16_t> &board_index, int nounce)
{
   //print( "BlockCoined::delete_marked_then_fill\n ");
   //print( "BlockCoined::delete_marked_then_fill before erase");
   /*print_vector_bool(mark_del);
   print( "\n ");
   print_vector(temp_vec);
   print( "\n ");
    print("index in before erase is:");
        for_each(board_index.begin(),board_index.end(), [](uint16_t b){print(" ", std::to_string(b));});
        print("\n");*/
   int row_index = 0;
   for_each(mark_del.begin(), mark_del.end(), [&](bool del) {
      if (del)
      {
         // std :: vector :: erase function call
         // erase the first 3 elements of vector vector
         //cout << row_index;
         temp_vec.erase(temp_vec.begin() + row_index, temp_vec.begin() + row_index + 1);
         board_index.erase(board_index.begin() + row_index, board_index.begin() + row_index + 1);
         //board_index.erase(temp_vec.begin()+row_index, temp_vec.begin() + row_index + 1);
         row_index--;
      }
      row_index++;
   });

   //print view_row
   //cout << '\n';
   //for_each(temp_vec.begin(), temp_vec.end(), [](char a) { cout << a << ' '; });
   //cout << '\n';
   //cout << num - temp_vec.size() << '\n';
   std::string sudo_seed = "";
   char sudo_seed_char = 'f';
   //insert random char
   int push_count = num - temp_vec.size();

   sudo_seed = ran_char(num_unique_char, sudo_seed_char, push_count, nounce);
   //print("psueudo seed is: ");
   uint16_t index = current_game.board_index_value;
   for (int i = 0; i < push_count; i++)
   {
      index++;
      print(sudo_seed[i + 1]);
      temp_vec.insert(temp_vec.begin(), sudo_seed[i + 1]);
      board_index.insert(board_index.begin(), index);
      //print("index is: ", std::to_string(current_game.board_index_value++));
   }
   current_game.board_index_value = index;
   /*print("\n");
  print("index in del  is:");
        for_each(board_index.begin(),board_index.end(), [](uint16_t b){print(" ", std::to_string(b));});
        print("\n");*/
   //for_each(temp_vec.begin(), temp_vec.end(), [](char a) { cout << a << ' '; });
   //cout << '\n';
   //print("pushed str: ", sudo_seed);
   //print( "BlockCoined::delete_marked_then_fill after erase");
   // print_vector(temp_vec);
   //print( "\n ");
}

void BlockCoined::print_view(array_view &view)
{
   //cout << '\n';
   //cout << "view is: ";
   //for_each(view.begin(), view.end(), [](char b){cout<<b<<' ';}
   // );
   //cout << '\n';
}

void BlockCoined::print_vector(vector<char> &view)
{
   print("\n vector is: ");
   //cout << "vector is: ";
   for_each(view.begin(), view.end(), [](char b) { print(b); });
   print("\n ");
}

void BlockCoined::print_vector_bool(const vector<bool> &view)
{
   print("\n vector bool is: ");
   for_each(view.begin(), view.end(), [](bool b) { print(" ", b); });
   print("\n ");
}

void BlockCoined::print_board(boost::multi_array<char, 2> &a)
{
   /*std::cout<<"\n______________________________________________________\n";
  std::cout << '\n';

  array_view view_col = a[boost::indices[a_range][0]];
  array_view view_row = a[boost::indices[0] [a_range]];

  //a[boost::indices[a_range][1]] = view_col;

  //Print 8x8 Block to Console
  int block_label = 0;
  for (int i=0; i<num; i++){
    array_view view = a[boost::indices[i][a_range]];
    for_each(view.begin(), view.end(), [&](char b) {
      block_label++;
      cout << b <<' ';
  if (block_label < 10){
   //cout << '0';
  }
      //cout << block_label <<' ';
    });
    std::cout << '\n';
  }

  //For get view of selected row or column
  std::cout<<"\n______________________________________________________\n";*/
}

void BlockCoined::compare_vec(vector<char> &V1, vector<char> &V2, vector<bool> &result_diff)
{
   //vector<bool> result_diff(num, false);
   print("compare_vec\n ");
   int index = 0;
   for_each(V1.begin(), V1.end(), [&](char b) {if (V1[index] != V2[index]){
    result_diff[index]=1;}index++; });
}

void BlockCoined::process_vector_queue(std::queue<view_pos> &vector_queue, block_jewel &current_game, int &score_tot, animaindex &animation_index_table, const int &anima_index, const name &anima_payer)
{
   vector<char> temp_vec1(num, 0);           //original
   vector<char> temp_vec2(num, 0);           //processed
   vector<uint16_t> temp_vec1_index(num, 0); //original
   vector<uint16_t> temp_vec2_index(num, 0); //processed
   vector<bool> compare_result(num, 0);
   vector<bool> deleted_result(num, 0);

   int count = 0;
   int score_single_pass = 0;
   int index = 0;

   //animation tracking logic
   unsigned int row_index;
   unsigned level;
   //check if this is the first animation emplace
   if (animation_index_table.begin() != animation_index_table.end())
   {
      auto itr_anima = animation_index_table.end(); //last level
      itr_anima--;
      row_index = itr_anima->index;
      level = itr_anima->level + 1; //level of nested vector diff check
   }                                //increment from last index
   else
   { //if it is first
      auto itr_anima = animation_index_table.begin();
      row_index = 0;                //if first in the table initiate index
      level = itr_anima->level + 1; //level of nested vector diff check
   }

   print("queue processing 1\n ");
   if (!vector_queue.empty())
   {
      view_pos current_process = vector_queue.front();
      if (current_process.row_or_column)
      {
         //array_view view_row = board[boost::indices[current_process.number - 1] [a_range]];
         //current_game.board[current_process.number - 1]
         //copy view into vector
         /*std::memcpy(&temp_vec2[0], &current_game.board[(current_process.number - 1) * num], num);
        std::memcpy(&temp_vec2_index[0], &current_game.board_index[(current_process.number - 1) * num ], num);*/
         index = 0;
         for_each(temp_vec2.begin(), temp_vec2.end(), [&](char b) {
           temp_vec2[index]= current_game.board[index + num*(current_process.number - 1)];
           temp_vec2_index[index]= current_game.board_index[index + num*(current_process.number - 1)];
           index++; });
         //print("row index before func into board is:");
         //for_each(temp_vec2_index.begin(),temp_vec2_index.end(), [](uint16_t b){print(" ", std::to_string(b));});
         //print("\n");
         /*
        print( "queue processing copied board is : ");
        print_vector(current_game.board);
        print( "\n ");
         print( "queue processing copied row vector is : ");
           print_vector(temp_vec2);
           print( "\n ");*/
         //save pre-processed vector
         std::memcpy(&temp_vec1[0], &temp_vec2[0], num);
         score_single_pass = check_score(deleted_result, temp_vec2);
         while (score_single_pass > 0)
         {
            //print_vector(temp_vec2);
            score_tot = score_tot + score_single_pass;
            delete_marked_then_fill(current_game, deleted_result, temp_vec2, temp_vec2_index, score_tot);
            //check_delete_and_fill( temp_vec2, score_single_pass , deleted_result);
            //print("delete result ", std::to_string(delete_result));
            print(" \n ");
            score_single_pass = check_score(deleted_result, temp_vec2);
            print_vector(temp_vec2);
            //insert into animation table
            row_index++;
            animation_index_table.emplace(anima_payer, [&](auto &rowindex) {
               rowindex.index = row_index;
               rowindex.level = level;
               if (current_process.row_or_column)
               {
                  rowindex.orientation = 0;
               }
               else
               {
                  rowindex.orientation = 1;
               }
               rowindex.pos_num = current_process.number;
               std::memcpy(&rowindex.view[0], &temp_vec2[0], num);
               index = 0;
               for_each(temp_vec2_index.begin(), temp_vec2_index.end(), [&](uint16_t b) {
                rowindex.board_index[index]= b;
           index++; });
               //std::memcpy(&rowindex.board_index[0], &temp_vec2_index[0], num);
               int counter = 0;

               for_each(deleted_result.begin(), deleted_result.end(), [&](bool del) {
                  //print(std::to_string(del));
                  if (del)
                  {
                     rowindex.empty[counter] = '@';
                  }
                  counter++;
               });
            });
         }
         //copy back into board
         /*print("index before merging into board is:");
        for_each(temp_vec2_index.begin(),temp_vec2_index.end(), [](uint16_t b){print(" ", std::to_string(b));});
        print("\n");*/
         index = 0;
         for_each(temp_vec2.begin(), temp_vec2.end(), [&](char b) {
           current_game.board[index + num*(current_process.number - 1)] = temp_vec2[index];
           current_game.board_index[index + num*(current_process.number - 1)] = temp_vec2_index[index];
           index++; });
         //std::memcpy(&current_game.board[(current_process.number - 1) * num ], &temp_vec2[0], num);
         //std::memcpy(&current_game.board_index[(current_process.number - 1) * num ], &temp_vec2_index[0], num);
         //save into board
         //board[boost::indices[current_process.number - 1] [a_range]] = view_row;
         vector_queue.pop();
         //print_view(view_row);
         //print( "view row is: ");
         print("queue processing 2\n ");
         //print( "\n ");
      }
      else
      {
         index = 0;
         //array_view view_col = board[boost::indices[a_range][current_process.number - 1]];
         //copy board column to vector
         for_each(temp_vec2.begin(), temp_vec2.end(), [&](char b) {
           temp_vec2[index]= current_game.board[index*num + current_process.number - 1];
           temp_vec2_index[index]= current_game.board_index[index*num + current_process.number - 1];
           index++; });
         //print("column index before func into board is:");
         //for_each(temp_vec2_index.begin(),temp_vec2_index.end(), [](uint16_t b){print(" ", std::to_string(b));});
         //print("\n");
         /*print( "queue processing copied column vector is : ");
           print_vector(temp_vec2);
           print( "\n ");*/
         //save pre-processed vector
         std::memcpy(&temp_vec1[0], &temp_vec2[0], num);
         score_single_pass = check_score(deleted_result, temp_vec2);

         while (score_single_pass > 0)
         {
            print_vector(temp_vec2);
            score_tot = score_tot + score_single_pass;
            delete_marked_then_fill(current_game, deleted_result, temp_vec2, temp_vec2_index, score_tot);
            //check_delete_and_fill( temp_vec2, score_single_pass, deleted_result);
            score_single_pass = check_score(deleted_result, temp_vec2);
            //print_vector(temp_vec2);
            //insert into animation table
            row_index++;
            animation_index_table.emplace(anima_payer, [&](auto &rowindex) {
               rowindex.index = row_index;
               rowindex.level = level;
               if (current_process.row_or_column)
               {
                  rowindex.orientation = 0;
               }
               else
               {
                  rowindex.orientation = 1;
               }
               rowindex.pos_num = current_process.number;
               std::memcpy(&rowindex.view[0], &temp_vec2[0], num);
               index = 0;
               for_each(temp_vec2_index.begin(), temp_vec2_index.end(), [&](uint16_t b) {
                rowindex.board_index[index]= b;
           index++; });
               //std::memcpy(&rowindex.board_index[0], &temp_vec2_index[0], num);
               int counter = 0;

               for_each(deleted_result.begin(), deleted_result.end(), [&](bool del) {
                  //print(std::to_string(del));
                  if (del)
                  {
                     rowindex.empty[counter] = '@';
                  }
                  counter++;
               });
            });
         }
         //copy vector to view
         /*print("index before merging into board is:");
        for_each(temp_vec2_index.begin(),temp_vec2_index.end(), [](uint16_t b){print(" ", std::to_string(b));});
        print("\n");*/
         index = 0;
         for_each(temp_vec2.begin(), temp_vec2.end(), [&](char b) {
           current_game.board[index*num + current_process.number - 1] = b;
           current_game.board_index[index*num + current_process.number - 1] = temp_vec2_index[index] ;
           index++; });
         //may refactor into function
         //std::memcpy(&view_col[0], &temp_vec2[0], num);
         //board[boost::indices[a_range] [current_process.number - 1]] = view_col;
         vector_queue.pop();
         print("queue processing 3\n ");
         //print_view(view_col);
      }

      //compare vectors
      print("queue processing 4\n ");
      //push changes rows or columns into queue
      int index = 0;
      int count = 0;
      compare_vec(temp_vec1, temp_vec2, compare_result);

      for_each(compare_result.begin(), compare_result.end(), [&](bool b) {
      if (b){
        count++;
        view_pos position;
        position.row_or_column = !current_process.row_or_column;
        position.number = index + 1;
        if (index < num){
        vector_queue.push(position);
        }
      }
      index++; });
      print("queue processing 6\n ");
      //process_vector_queue(vector_queue, board, score_tot);
      return;
      //process view below
   }
   else
   {

      return; //done
   }
}

/**
 * @brief Check if cell is empty
 * @param cell - value of the cell (should be either 0, 1, or 2)
 * @return true if cell is empty
 */
bool is_empty_cell(const uint8_t &cell)
{
   return cell == 0;
}

/**
 * @brief Check for valid movement
 * @detail Movement is considered valid if it is inside the board and done on empty cell
 * @param row - the row of movement made by the player
 * @param column - the column of movement made by the player
 * @param board - the board on which the movement is being made
 * @return true if movement is valid
 */
bool is_valid_movement(const uint16_t &cell_cicked1, const uint16_t &cell_cicked2)
{
   if (cell_cicked1 < 64)
   {
      if (cell_cicked2 < 64)
      {
         if (std::abs(cell_cicked1 - cell_cicked2) == 1 || std::abs(cell_cicked1 - cell_cicked2) == 8)
         {
            return true;
         }
      }
      //uint32_t movement_location = row * BlockCoined::game::board_width + column;
      //bool is_valid = movement_location < board.size() && is_empty_cell(board[movement_location]);
   }
   return false;
}
//boot strap tester
int BlockCoined::get_score(const name &host, const name &challenger, BlockCoined::block_jewel &current_game, const uint16_t &cell_cicked1, const uint16_t &cell_cicked2)
{
   animaindex animation_index_table(_self, host.value);
   auto itr_temp = animation_index_table.begin();
   while (itr_temp != animation_index_table.end())
   {
      itr_temp = animation_index_table.erase(itr_temp);
   }
   auto itr_anima = animation_index_table.begin();

   int score_tot1 = 0;
   BlockCoined::view_pos P1;
   BlockCoined::view_pos P2;
   BlockCoined::view_pos P3;

   if (abs(cell_cicked1 - cell_cicked2) > 1)
   {
      P1.row_or_column = true;
      P2.row_or_column = true;
      P3.row_or_column = false;
      P1.number = cell_cicked1 / this->num + 1;
      P2.number = cell_cicked2 / this->num + 1;
      P3.number = cell_cicked1 % this->num + 1;
      BlockCoined::view_queue.push(P3);
      BlockCoined::view_queue.push(P1);
      BlockCoined::view_queue.push(P2);
   }
   else
   {
      P1.row_or_column = true;
      P2.row_or_column = false;
      P3.row_or_column = false;
      P1.number = cell_cicked1 / this->num + 1;
      P2.number = cell_cicked1 % this->num + 1;
      P3.number = cell_cicked2 % this->num + 1;
      BlockCoined::view_queue.push(P1);
      BlockCoined::view_queue.push(P2);
      BlockCoined::view_queue.push(P3);
   }

   int anima_index_being_processed = 1; //first value gets pushed into animation table will have a value of index = index+1;
   while (!view_queue.empty())
   {

      print("in get score\n ");
      //transform(current_game.board);
      //print( "in transformed\n ");
      process_vector_queue(view_queue, current_game, score_tot1, animation_index_table, anima_index_being_processed, challenger);
      print("queue processed\n ");
      //inverse_transform(current_game.board);
      //print( "inverse_transform\n ");

      anima_index_being_processed++; //track index being processed b/c queue does not
   }

   return score_tot1;
}
/**
 * @brief Get winner of the game
 * @detail Winner of the game is the first player who made three consecutive aligned movement
 * @param current_game - the game which we want to determine the winner of
 * @return winner of the game (can be either none/ draw/ account name of host/ account name of challenger)
 */
name BlockCoined::get_winner(const BlockCoined::block_jewel &current_game, const name &host)
{
   name winner = "none"_n;
   if (current_game.Game_turn >= current_game.Queue_pos_max)
   {
      ticketlist existing_ticket_list(_self, host.value);

      block_jewel::chalindex chal_index_table(_self, host.value);
      int prev_score = 0;
      name prev_chal = "none"_n;
      for_each(chal_index_table.begin(), chal_index_table.end(), [&](auto &challenger) {
         if (challenger.score > prev_score)
         {
            winner = challenger.challenger;
         }
         prev_score = challenger.score;
         auto itr_ticket = existing_ticket_list.find(challenger.challenger.value); //for each challenger look up score and add score to current ticket holding
         bool no_tickets_yet = (itr_ticket == existing_ticket_list.end());
         eosio_assert(!(no_tickets_yet), "game error: missing challenger in ticket_list");
         existing_ticket_list.modify(itr_ticket, _self, [&](auto &entry) {
            entry.num_ticket += (current_game.Queue_pos_max - 1) * challenger.score / current_game.host_score; //will use num_ticket_available as holding
         }); });
   }
   // Draw if the board is full, otherwise the winner is not determined yet
   return winner;
}

/**
 * @brief Apply create action
 */
/*void BlockCoined::create(const name& challenger, const name& host) {
      print( "created jgames: \n ");
   //eosio::print("here");
   require_auth(host);
   //eosio_assert(challenger != host, "challenger shouldn't be the same as host");
   // Check if game already exists
   jgames existing_host_games( _self, _self.value ); //constructor
   //auto itr = existing_host_games.find( challenger.value );
   //eosio_assert(itr == existing_host_games.end(), "game already exists");

   existing_host_games.emplace(host, [&]( auto& g ) {
      g.host = host;
      g.turn = host;
   });
}  */

/**
 * @brief Apply create action
 */
void BlockCoined::createjgames(const name &host)
{
   print("created jgames: \n ");
   //eosio::print("here");
   require_auth(host);
   //eosio_assert(challenger != host, "challenger shouldn't be the same as host");
   // Check if game already exists
   jgames existing_host_games(_self, host.value); //constructor

   //auto itr = existing_host_games.find( challenger.value );
   //eosio_assert(itr == existing_host_games.end(), "game already exists");

   existing_host_games.emplace(host, [&](auto &g) {
      g.host = host;
      g.turn = host;

      block_jewel::chalindex chal_index_table(_self, host.value);
      auto itr_temp = chal_index_table.find(host.value);
      if (itr_temp == chal_index_table.end())
      {
         print("got here 8 \n");
         auto itr_index = chal_index_table.emplace(host, [&](auto &index) {
            index.score = 0;
            index.challenger = host;
         });
      }
      //test place host into first queue
      block_jewel::queue_table queue_index_table(_self, host.value);
      auto itr_temp2 = queue_index_table.find(1);
      if (itr_temp2 == queue_index_table.end())
      {
         auto itr_index2 = queue_index_table.emplace(host, [&](auto &index) {
            index.index = 1;
            index.challenger = host;
         });
      }
   });

   gamelist existing_game_list(_self, _self.value); // scope to the contract name
   existing_game_list.emplace(host, [&](auto &list) {
      list.host = host;
      list.tot_num_ticket = 0;
   });
}

void BlockCoined::joinhostgame(const name &host, const name &challenger)
{
   print("joining host_game: \n ");
   //print( "challenger name is: \n ");
   print("challenger name is: ", challenger);
   print("\n ");
   //eosio::print("here");
   //require_auth(challenger);

   eosio_assert(challenger != host, "challenger shouldn't be the same as host");
   // Check if game already exists

   jgames existing_host_games(_self, host.value); //constructor

   auto itr = existing_host_games.find(host.value);
   //auto itr = existing_host_games.find( challenger.value );
   //eosio_assert(itr == existing_host_games.end(), "game already exists");

   existing_host_games.modify(itr, itr->host, [&](auto &g) {
      print("got here 3 \n");
      block_jewel::chalindex chal_index_table(_self, host.value);
      auto itr_temp = chal_index_table.find(challenger.value);
      if (itr_temp == chal_index_table.end())
      {
         print("got here 7 \n");
         auto itr_index = chal_index_table.emplace(challenger, [&](auto &index) {
            index.score = 0;
            index.challenger = challenger;
         });
      }
   });
   ticketlist existing_ticket_list(_self, host.value);
   auto itr_ticket = existing_ticket_list.find(challenger.value);
   bool no_tickets_yet = (itr_ticket == existing_ticket_list.end());
   if (no_tickets_yet)
   {                                                         //contract pays
      existing_ticket_list.emplace(_self, [&](auto &entry) { //add new row to table
         entry.challenger = challenger;
         entry.num_ticket = 0;
      });
   }
}

/**
 * @brief Apply restart action
 */
/*void BlockCoined::restart(const name &challenger, const name &host, const name &by)
{
   print("restarted \n ");
   require_auth(by);

   // Check if game exists
   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(challenger.value);
   eosio_assert(itr != existing_host_games.end(), "game doesn't exists");

   // Check if this game belongs to the action sender
   eosio_assert(by == itr->host, "this is not your game!");

   // Reset game
   existing_host_games.modify(itr, itr->host, [](auto &g) {
      g.reset_game();
   });
}*/

/**
 * @brief Apply close action
 */
void BlockCoined::close(const name &challenger, const name &host)
{
   print("closed \n ");
   require_auth(host);

   // Check if game exists
   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);
   print("Got here in close \n ");
   eosio_assert(itr != existing_host_games.end(), "game doesn't exists");
   existing_host_games.erase(itr);

   //auto it = existing_host_games.begin();
   //while (it != existing_host_games.end()) {
   //  it = existing_host_games.erase(it);
   //}

   block_jewel::chalindex chal_index_table(_self, host.value);
   auto itr1 = chal_index_table.begin();
   while (itr1 != chal_index_table.end())
   {
      itr1 = chal_index_table.erase(itr1);
   }
   //eosio_assert(itr1 != chal_index_table.end(), "challenger doesn't exists");
   animaindex animation_index_table(_self, host.value);
   auto itr_temp = animation_index_table.begin();
   while (itr_temp != animation_index_table.end())
   {
      itr_temp = animation_index_table.erase(itr_temp);
   }

   gamelist existing_game_list(_self, _self.value); // scope to the contract name
   auto itr_game_lst = existing_game_list.find(host.value);
   existing_game_list.erase(itr_game_lst);

   block_jewel::queue_table queue_index_table(_self, host.value);
   auto itr_temp2 = queue_index_table.begin();
   while (itr_temp2 != queue_index_table.end())
   {
      itr_temp2 = queue_index_table.erase(itr_temp2);
   }
   // Remove game
   //existing_host_games.erase(itr);
   //chal_index_table.erase(itr1);
   ticketlist existing_ticket_list(_self, host.value);
   auto itr_ticket = existing_ticket_list.begin();
   while (itr_ticket != existing_ticket_list.end())
   {
      int qty = itr_ticket->num_ticket - 1;
      asset amount(qty * 10000, eosio::symbol("BLC", 4));
      existing_ticket_list.modify(itr_ticket, _self, [&](auto &entry) { //move paid token to exchange for queues
         entry.num_ticket = 0;                                          //will use num_ticket_available as holding
      });

      print("account being refunded is: ", itr_ticket->challenger);
      if (qty > 0)
      {
         action(
             permission_level{get_self(), "active"_n},
             "blocointoken"_n,
             "transfer"_n,
             std::make_tuple(_self, itr_ticket->challenger, amount, std::string("test")))
             .send();
      }
      itr_ticket = existing_ticket_list.erase(itr_ticket);
   }
}

/**
 * @brief Apply move action
 */
void BlockCoined::move(const name &host, const name &by, const uint16_t &cell_cicked1, const uint16_t &cell_cicked2)
{
   print("move \n ");
   require_auth(by);
   // Check if game exists
   jgames existing_host_games(_self, host.value);
   const auto itr = existing_host_games.find(host.value);
   print("move first Queue_pos is: ", std::to_string(itr->Queue_pos));
   print("\n ");
   eosio_assert(itr != existing_host_games.end(), "game doesn't exists");
   // Check if game state is ready
   eosio_assert(check_set_game_state(host) == 1, "game is not ready to play");
   // Check if by is the current player on Queue
   name queued_name = get_player_from_queue(*itr, host);
   print("player queued name: ", queued_name);
   eosio_assert(by == queued_name, "it's not your turn yet!");
   // Check if this game hasn't ended yet
   eosio_assert(itr->winner == "none"_n, "the game has ended!");

   eosio_assert(is_valid_movement(cell_cicked1, cell_cicked2), "invalid move");
   // Check if this game belongs to the action sender
   //eosio_assert(by == itr->host, "this is not your game!");
   // Check if this is the  action sender's turn
   //eosio_assert(by == itr->turn, "it's not your turn yet!");
   existing_host_games.modify(itr, itr->host, [&](auto &g) {
      g.state = 2; //set game to run
   });

   if (itr->Game_turn < itr->Queue_pos_max)
   {
      //Run game
      existing_host_games.modify(itr, itr->host, [&](auto &g) {
         g.Game_turn++;
         g.turn = by; //set game to run
         g.block_num = tapos_block_num();
         print("g.Game_turn is: ", std::to_string(g.Game_turn));
         print("\n ");
         print("g.block_num is: ", std::to_string(g.block_num));
         print("\n ");
      });
      // Check if user makes a valid movement
      //eosio_assert(is_valid_movement(row, column, itr->board), "not a valid movement!");

      // Fill the cell, 1 for host, 2 for challenger
      // const uint8_t cell_value = itr->turn == itr->host ? 1 : 2;
      //const auto turn = itr->turn == itr->host ? itr->challenger : itr->host;

      char temp = 'A';
      int idx;
      int score = 0;

      existing_host_games.modify(itr, itr->host, [&](auto &g) {
         //swap colors
         temp = g.board[cell_cicked1];
         idx = g.board_index[cell_cicked1];
         g.board[cell_cicked1] = g.board[cell_cicked2];
         g.board_index[cell_cicked1] = g.board_index[cell_cicked2];
         g.board[cell_cicked2] = temp;
         g.board_index[cell_cicked2] = idx;
         //get_score send clicks, move and delete cells according to board rules, and returns score
         score = get_score(host, by, g, cell_cicked1, cell_cicked2);
         g.host_score = g.host_score + score;
         //g.Queue_pos = g.Queue_pos  + 1 ;
         print("g.Queue_pos is: ", std::to_string(g.Queue_pos));
         print("\n ");
         set_score(host, by, score);
         print("score is: ", std::to_string(g.host_score));
         print("\n ");
         pop_queue(host);

         //g.board[row * BlockCoined::game::board_width + column] = cell_value;
         //g.turn = turn;
         //g.winner = get_winner(g);
      });
   }
   // print("score is: ", std::to_string(g.host_score));
   if (itr->Game_turn == itr->Queue_pos_max)
   {
      print("game ended");
      if (itr->winner == "none"_n)
      {
         name winner = get_winner(*itr, host);
         print("winner is: ", winner);
         existing_host_games.modify(itr, itr->host, [&](auto &g) { g.winner = winner; });
      }
   }
}

//action
void BlockCoined::stealturn(const name &host, const name &by)
{
   print("exec stealturn");
   print("\n ");
   require_auth(by);
   block_jewel::chalindex chal_index_table(_self, host.value);
   auto itr_temp = chal_index_table.find(by.value);
   eosio_assert(itr_temp != chal_index_table.end(), "You are not in the game");
   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);
   eosio_assert(tapos_block_num() - itr->block_num >= itr->time_out_limit, "Not enough block passed from last move to steal yet");

   existing_host_games.modify(itr, itr->host, [&](auto &g) {
      block_jewel::queue_table queue_index_table(_self, host.value);
      auto q_itr = queue_index_table.begin();
      queue_index_table.modify(q_itr, by, [&](auto &q) { q.challenger = by; });
      g.turn = by;
   });
}

vector<bool> BlockCoined::mark_del(array_view &view_line, int board_width)
{
   //mark delete mark
   int k = 0;
   int scored = 0;
   vector<bool> mark_del(board_width, 0);

   for (i = 1; i < board_width - 1; i++)
   {
      if (view_line[i] == view_line[i - 1])
      {
         if (view_line[i] == view_line[i + 1])
         {
            scored++;
            mark_del[i] = 1;
            mark_del[i - 1] = 1;
            mark_del[i + 1] = 1;
            //cout << "found" << '\n';
            for (k = i + 2; k < board_width - 1; k++)
            {
               if (view_line[i] == view_line[k])
               {
                  scored++;
                  mark_del[k] = 1;
                  //cout << "found!" << '\n';
               }
               else
               {
                  break;
               }
            }
            if (scored > 1)
            {
               i = k;
            }
         }
      }
   }

   return mark_del;
}

void BlockCoined::transform(const vector<char> &board)
{
   int j;
   int k;
   for (int i = 0; i < board.size(); i++)
   {
      j = i % this->board_width;
      k = i / this->board_width;
      this->board[k][j] = board[i];
   }
}

void BlockCoined::inverse_transform(vector<char> &board)
{
   int j;
   int k;
   for (int i = 0; i < board.size(); i++)
   {
      j = i % this->board_width;
      k = i / this->board_width;
      board[i] = this->board[k][j];
   }
}

BlockCoined::array_view BlockCoined::get_view(bool row_or_col, int index)
{

   if (row_or_col)
   {
      range a_range = range().start(0).finish(this->board_width).stride(1);
      array_view view_row = this->board[boost::indices[index][a_range]];
      return view_row;
   }
   else
   {
      range a_range = range().start(0).finish(this->board_height).stride(1);
      array_view view_col = this->board[boost::indices[a_range][index]];
      return view_col;
   }
}

void BlockCoined::set_board_from_view(const BlockCoined::array_view view, bool row_or_col, int index)
{

   if (row_or_col)
   {
      range a_range = range().start(0).finish(this->board_width).stride(1);
      this->board[boost::indices[index][a_range]] = view;
   }
   else
   {
      range a_range = range().start(0).finish(this->board_height).stride(1);
      this->board[boost::indices[a_range][index]] = view;
   }
}

name BlockCoined::get_player_from_queue(const block_jewel &current_game, const name &host)
{
   block_jewel::queue_table queue_index_table(_self, host.value);
   auto itr = queue_index_table.begin();
   print("get_player_from_queue is: ", std::to_string(current_game.Queue_pos));
   print("\n ");
   print("get_player_from_queue 2 is: ", std::to_string(itr->index));
   print("\n ");
   //if (current_game.Queue_pos == itr->index){
   return (itr->challenger);
   //}else{return("none"_n);}
}

int BlockCoined::check_set_game_state(const name &host)
{
   int new_game_state = 0;
   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);
   if (is_queue_empty(host))
   {
      new_game_state = 0;
   }
   else
   {
      new_game_state = 1;
   }
   //set current game state
   existing_host_games.modify(itr, itr->host, [&](auto &g) {
      g.state = new_game_state;
   });
   return new_game_state;
}

bool BlockCoined::is_queue_empty(const name &host)
{
   block_jewel::queue_table queue_index_table(_self, host.value);
   auto begin_itr = queue_index_table.begin();
   auto end_itr = queue_index_table.end();
   if (begin_itr == end_itr)
   {
      return true;
   }
   else
   {
      return false;
   }
}

void BlockCoined::set_score(const name &host, const name &by, unsigned int score)
{
   block_jewel::chalindex chal_index_table(_self, host.value);
   auto itr_temp = chal_index_table.find(by.value);
   if (itr_temp != chal_index_table.end())
   {
      chal_index_table.modify(itr_temp, itr_temp->challenger, [&](auto &index) {
         index.score = index.score + score;
      });
   }
}

void BlockCoined::enqueue(const name &host, const name &buyer)
{
   print("in enqueue ");
   print("\n ");
   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);
   existing_host_games.modify(itr, host, [&](auto &g) {
      g.Queue_pos = g.Queue_pos + 1;
   });

   block_jewel::queue_table queue_index_table(_self, host.value);
   auto end_itr = queue_index_table.end();
   auto begin_itr = queue_index_table.begin();

   queue_index_table.emplace(buyer, [&](auto &index) {
      int temp_index;

      index.index = itr->Queue_pos;
      index.challenger = buyer;
   });
}

void BlockCoined::pop_queue(const name &host)
{
   print("in pop_queue ");
   print("\n ");
   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);
   existing_host_games.modify(itr, itr->host, [&](auto &g) {
      block_jewel::queue_table queue_index_table(_self, host.value);
      auto begin_itr = queue_index_table.begin();
      //begin_itr++;
      //check top of the table is synced with queue
      print("in pop, begin index is: ", std::to_string(begin_itr->challenger.value));
      print("g.turn is: ", std::to_string(g.turn.value));
      //if(g.turn == begin_itr->challenger ){
      queue_index_table.erase(begin_itr);
      //}
      g.turn = "none"_n;
   });
}

void BlockCoined::transfer_action(const name buyer, const name reciever, const asset quantity, const string memo)
{
   if (buyer == _self)
      return;
   name host = eosio::name(memo);
   asset amount(1, eosio::symbol("BLC", 4));
   eosio_assert(quantity.symbol == amount.symbol, "please send BLC token to play");
   int quant = quantity.amount / 10000; //unit is int 1000s regardless of decimal place
   require_auth(buyer);
   print("host name is ", host);
   block_jewel::chalindex chal_index_table(_self, host.value);
   auto itr_temp = chal_index_table.find(buyer.value);
   //print( "in buy queue itr is, ", std::to_string(itr_temp));
   eosio_assert(itr_temp != chal_index_table.end(), "please join game first");
   //print( "in buy queue itr2 is, ", std::to_string(chal_index_table.end()));
   print("\n ");
   //By pass this since assert was forced
   if (itr_temp == chal_index_table.end())
   {
      print("join game called");
      print("\n ");
      joinhostgame(host, buyer);
   }

   //after allocated payment to this contract

   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);

   print("in buy queue, Queue_pos_max is ", std::to_string(itr->Queue_pos_max));
   print("in buy queue, Queue_pos is ", std::to_string(itr->Queue_pos));
   print("in buy queue, quant is  ", std::to_string(quant));
   print("\n ");
   eosio_assert(itr->Queue_pos_max - itr->Queue_pos >= quant, "Queue full for this game");

   //insert into ticket take. Having tickets will allow players to buy Queues
   ticketlist existing_ticket_list(_self, host.value);
   auto itr_ticket = existing_ticket_list.find(buyer.value);
   bool no_tickets_yet = (itr_ticket == existing_ticket_list.end());
   if (no_tickets_yet)
   {                                                         //contract pays
      existing_ticket_list.emplace(_self, [&](auto &entry) { //add new row to table
         entry.challenger = buyer;
         entry.num_ticket = quant;
      });
   }
   else
   {
      existing_ticket_list.modify(itr_ticket, _self, [&](auto &entry) { //move paid token to exchange for queues
         entry.num_ticket = entry.num_ticket + quant;                   //top off existring number of queue tickets
      });                                                               //contract pays
                                                                        //check_set_game_state(host);
   }                                                                    //test function for transfer action listener
}
//action
//action: need challenger to spend RAM on adding queues (rows) to table
//buy queues with ticket user initiate this action
void BlockCoined::buyqueue(const name &host, const name &challenger, const uint16_t &quantity)
{
   require_auth(challenger);
   uint16_t num_ticket_available = 0;
   uint16_t quant = quantity;

   jgames existing_host_games(_self, host.value);
   auto itr = existing_host_games.find(host.value);
   eosio_assert(itr->Queue_pos_max - itr->Queue_pos >= quantity, "Queue full for this game");

   //update game list to show how many tickets are in the pot
   gamelist existing_game_list(_self, _self.value);
   auto itr_game_lst = existing_game_list.find(host.value);
   existing_game_list.modify(itr_game_lst, challenger, [&](auto &list) { list.tot_num_ticket += quantity; });

   //look up challenger and return how many tokens is left in game
   ticketlist existing_ticket_list(_self, host.value);
   auto itr_ticket = existing_ticket_list.find(challenger.value);
   bool no_tickets_yet = (itr_ticket == existing_ticket_list.end());

   eosio_assert(!(no_tickets_yet), "please buy send token into current game first");
   eosio_assert(itr_ticket->num_ticket >= quantity, "Not enough tickets available");

   num_ticket_available = itr_ticket->num_ticket;
   existing_ticket_list.modify(itr_ticket, _self, [&](auto &entry) { //move paid token to exchange for queues
      entry.num_ticket = entry.num_ticket - quantity;                //will use num_ticket_available as holding
   });

   while (quant > 0)
   {
      print("in while loop: ", std::to_string(quant));
      print("\n ");
      enqueue(host, challenger);
      quant--;
   }
}

//action
void BlockCoined::refundtoken(const name host, const name challenger)
{
   //transfer available tickets from ticketlist back into token form
   //INLINE_ACTION token transfer
   uint16_t qty;
   std::string quantity = "25.0000 BLC";

   ticketlist existing_ticket_list(_self, host.value);
   auto itr_ticket = existing_ticket_list.find(challenger.value);
   bool no_tickets_yet = (itr_ticket == existing_ticket_list.end());
   eosio_assert(!(no_tickets_yet), "nothing to refund");
   qty = itr_ticket->num_ticket - 1;

   asset amount(qty * 10000, eosio::symbol("BLC", 4));
   existing_ticket_list.modify(itr_ticket, _self, [&](auto &entry) { //move paid token to exchange for queues
      entry.num_ticket = 0;                                          //will use num_ticket_available as holding
   });
   existing_ticket_list.erase(itr_ticket);
   print("got here before refunding");
   print("receiver should be :", challenger.value);
   print("blockcoined is :", name("blockcoined").value);
   action(
       permission_level{get_self(), "active"_n},
       "blocointoken"_n,
       "transfer"_n,
       std::make_tuple(_self, challenger, amount, std::string("test")))
       .send();
}

extern "C" void apply(uint64_t receiver, uint64_t code, uint64_t action)
{
   auto self = receiver;
   if (action == name("transfer").value && code == name("blocointoken").value)
   {
      print("apply executed");
      print("receiver is :", receiver);
      print("blockcoined is :", name("blockcoined").value);
      eosio_assert(code == "blocointoken"_n.value, "Must transfer BLC");
      execute_action<BlockCoined>(eosio::name(receiver), eosio::name(code),
                                  &BlockCoined::transfer_action);
   }
   else if (code == receiver)
   {
      switch (action)
      {
         EOSIO_DISPATCH_HELPER(BlockCoined, (close)(move)(createjgames)(joinhostgame)(buyqueue)(stealturn)(refundtoken));
      }
   }
}
//EOSIO_DISPATCH( BlockCoined, (create)(restart)(close)(move)(createjgames)(joinhostgame)(buyqueue)(stealturn))


// Copyright (c) 2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "amount.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(amount_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(AmountTests) {
    BOOST_CHECK(CAmount(2) <= CAmount(2));
    BOOST_CHECK(CAmount(2) <= CAmount(3));

    BOOST_CHECK(CAmount(2) >= CAmount(2));
    BOOST_CHECK(CAmount(3) >= CAmount(2));

    BOOST_CHECK(CAmount(1) < CAmount(2));
    BOOST_CHECK(CAmount(-1) < CAmount(0));

    BOOST_CHECK(CAmount(2) > CAmount(1));
    BOOST_CHECK(CAmount(0) > CAmount(-1));

    BOOST_CHECK(CAmount(1) < CAmount(2));
    BOOST_CHECK(CAmount(-1) < CAmount(0));

    BOOST_CHECK(CAmount(2) > CAmount(1));
    BOOST_CHECK(CAmount(0) > CAmount(-1));

    BOOST_CHECK(CAmount(0) == CAmount(0));
    BOOST_CHECK(CAmount(0) != CAmount(1));

    CAmount amount(0);
    BOOST_CHECK_EQUAL(amount += CAmount(1), CAmount(1));
    BOOST_CHECK_EQUAL(amount += CAmount(-1), CAmount(0));
    BOOST_CHECK_EQUAL(amount -= CAmount(1), CAmount(-1));
    BOOST_CHECK_EQUAL(amount -= CAmount(-1), CAmount(0));

    BOOST_CHECK_EQUAL(COIN + COIN, CAmount(2 * COIN));
    BOOST_CHECK_EQUAL(2 * COIN + COIN, CAmount(3 * COIN));
    BOOST_CHECK_EQUAL(-1 * COIN + COIN, CAmount(0));

    BOOST_CHECK_EQUAL(COIN - COIN, CAmount(0));
    BOOST_CHECK_EQUAL(COIN - 2 * COIN, -1 * COIN);

    BOOST_CHECK_EQUAL(10 * CAmount(10), CAmount(100));
    BOOST_CHECK_EQUAL(-1 * CAmount(1), CAmount(-1));

    BOOST_CHECK_EQUAL(CAmount(10) / 3, CAmount(3));
    BOOST_CHECK_EQUAL(10 * COIN / COIN, 10.0);
    BOOST_CHECK_EQUAL(CAmount(10) / -3, CAmount(-3));
    BOOST_CHECK_EQUAL(-10 * COIN / (-1 * COIN), 10.0);

    BOOST_CHECK_EQUAL(CAmount(100) / 10, CAmount(10));
    BOOST_CHECK_EQUAL(CAmount(100) / 3, CAmount(33));
    BOOST_CHECK_EQUAL(CAmount(101) / 3, CAmount(33));

    BOOST_CHECK_EQUAL(CAmount(100) % 10, CAmount(0));
    BOOST_CHECK_EQUAL(CAmount(100) % 3, CAmount(1));
    BOOST_CHECK_EQUAL(CAmount(101) % 3, CAmount(2));
}

BOOST_AUTO_TEST_CASE(GetFeeTest) {
    CFeeRate feeRate;

    feeRate = CFeeRate(CAmount(0));
    // Must always return 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), CAmount(0));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e5), CAmount(0));

    feeRate = CFeeRate(CAmount(1000));
    // Must always just return the arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), CAmount(0));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), CAmount(1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), CAmount(121));
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), CAmount(999));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), CAmount(1000));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), CAmount(9000));

    feeRate = CFeeRate(CAmount(-1000));
    // Must always just return -1 * arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), CAmount(0));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), CAmount(-1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), CAmount(-121));
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), CAmount(-999));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), CAmount(-1000));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), CAmount(-9000));

    feeRate = CFeeRate(CAmount(123));
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), CAmount(0));
    // Special case: returns 1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), CAmount(1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), CAmount(1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), CAmount(14));
    BOOST_CHECK_EQUAL(feeRate.GetFee(122), CAmount(15));
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), CAmount(122));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), CAmount(123));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), CAmount(1107));

    feeRate = CFeeRate(CAmount(-123));
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), CAmount(0));
    // Special case: returns -1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), CAmount(-1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), CAmount(-1));

    // Check full constructor
    // default value
    BOOST_CHECK(CFeeRate(CAmount(-1), 1000) == CFeeRate(CAmount(-1)));
    BOOST_CHECK(CFeeRate(CAmount(0), 1000) == CFeeRate(CAmount(0)));
    BOOST_CHECK(CFeeRate(CAmount(1), 1000) == CFeeRate(CAmount(1)));
    // lost precision (can only resolve satoshis per kB)
    BOOST_CHECK(CFeeRate(CAmount(1), 1001) == CFeeRate(CAmount(0)));
    BOOST_CHECK(CFeeRate(CAmount(2), 1001) == CFeeRate(CAmount(1)));
    // some more integer checks
    BOOST_CHECK(CFeeRate(CAmount(26), 789) == CFeeRate(CAmount(32)));
    BOOST_CHECK(CFeeRate(CAmount(27), 789) == CFeeRate(CAmount(34)));
    // Maximum size in bytes, should not crash
    CFeeRate(MAX_MONEY, std::numeric_limits<size_t>::max() >> 1).GetFeePerK();
}

BOOST_AUTO_TEST_SUITE_END()


#include "Terrain.hpp"

#include "Object.hpp"

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <iostream>
#include <complex>

namespace
{
   std::ostream& operator << ( std::ostream& out, b2Vec2 const& vec )
   {
      out << std::complex<float>(vec.x,vec.y);
      return out;
   }

   b2PolygonDef m_tridef;
   std::ostream& operator << ( std::ostream& out, const b2PolygonDef& p )
   {
      unsigned last = p.vertexCount-1;
      float area = 0.f;
      float perimeter = 0.f;
      out << p.vertexCount;
      for( unsigned i=0; i<p.vertexCount; ++i )
      {
         area += b2Cross( p.vertices[last], p.vertices[i] );
         perimeter += (p.vertices[i] - p.vertices[last]).Length();
         last = i;
      }
      area *= 0.5;

      //out << " Area: " << area << " Permeter: " << perimeter;
      //out << " p/a: " << perimeter/area << " p^2/a: " << (perimeter*perimeter)/area;
      out << " p^2/a: " << (perimeter*perimeter)/area;
      return out;
   }
}

namespace ento
{
   namespace // anonymous
   {
      /// initialize and AABB so that any point expanding it will
      /// set upper and lower bounds to that value
      /// @param aabb
      inline void initAABB( b2AABB& aabb )
      {
         const float infinity = std::numeric_limits<float>::infinity();
         aabb.lowerBound.Set( infinity, infinity );
         aabb.upperBound.Set( -infinity, -infinity );
      }

      /// expand an AABB so that it encompasses vec
      /// @param aabb [IN-OUT] AABB to update
      /// @param vec point to encompass
      inline void expandAABB( b2AABB& aabb, const b2Vec2& vec )
      {
         const float epsilon = 0.1f;
         if( vec.x < aabb.lowerBound.x )
         {
            aabb.lowerBound.x = vec.x - epsilon;
         }
         if( vec.y < aabb.lowerBound.y )
         {
            aabb.lowerBound.y = vec.y - epsilon;
         }

         if( vec.x > aabb.upperBound.x )
         {
            aabb.upperBound.x = vec.x + epsilon;
         }
         if( vec.y > aabb.upperBound.y )
         {
            aabb.upperBound.y = vec.y + epsilon;
         }
      }

   } // anonymous namespace

   /// threshold formula:
   /// let A be equilateral triangle.
   /// min dist from centroid is b2_toiSlop
   /// centroid is 1/3 of height
   /// height ie 3 * b2_toiSlop
   /// tan(60) = height / (width/2)
   /// width = 2 * height / tan(60)
   /// width = 2 * height * sqrt(3) / 3
   /// width = 2 * b2_toiSlop * sqrt(3)
   /// width**2 = 4 * b2_toiSlop**2 * 3
   /// width**2 = 12 * b2_toiSlop**2
   /// width**2 = 12 * 0.04**2
   /// width**2 = 0.0195
   TerrainManager::Settings::Settings( void )
    : min_len_sqrd( 12 * b2_toiSlop * b2_toiSlop + 0.005f ), world(NULL)
   {
      ground_def.friction = 0.5f;
      ground_def.restitution = 0.5f;
      ground_def.vertexCount = 3;
   }

   TerrainManager::TerrainManager( const TerrainManager::Settings& settings )
    : m_obj(Object::e_Static_Terrain), m_world( settings.world ),
      m_colldet( settings.aabb, &m_colldet_cb ),
      m_tess( gluNewTess() ), m_tri_index(0),
      m_tridef( settings.ground_def ),
      m_min_len_sqrd( settings.min_len_sqrd )
   {
      if( ! m_tess )
         throw std::runtime_error( "out of memory for OpenGL" );

      gluTessNormal( m_tess, 0.0, 0.0, 1.0 );
      gluTessProperty( m_tess, GLU_TESS_WINDING_RULE, GLU_TESS_WINDING_POSITIVE );

      gluTessCallback( m_tess, GLU_TESS_BEGIN_DATA,
                       reinterpret_cast<void(*)()>(&TerrainManager::tess_begin) );

      gluTessCallback( m_tess, GLU_TESS_END_DATA,
                       reinterpret_cast<void(*)()>(&TerrainManager::tess_end) );

      gluTessCallback( m_tess, GLU_TESS_EDGE_FLAG_DATA,
                       reinterpret_cast<void(*)()>(&TerrainManager::tess_edge) );

      gluTessCallback( m_tess, GLU_TESS_VERTEX_DATA,
                       reinterpret_cast<void(*)()>(&TerrainManager::tess_vertex) );

      gluTessCallback( m_tess, GLU_TESS_COMBINE_DATA,
                       reinterpret_cast<void(*)()>(&TerrainManager::tess_combine) );

      gluTessCallback( m_tess, GLU_TESS_ERROR_DATA,
                       reinterpret_cast<void(*)()>(&TerrainManager::tess_error) );

      m_body = m_world->GetGroundBody();
      m_body->SetUserData( &m_obj );
   }

   TerrainManager::~TerrainManager( void )
   {
      gluDeleteTess(m_tess);
   }

   void TerrainManager::
   beginUpdate( void )
   {
      gluTessBeginPolygon( m_tess, this );
   }

   void TerrainManager::
   newVertex( const b2Vec2& v )
   {
      expandAABB( m_aabb, v );

      m_points.push_back( Vertex(v) );

      gluTessVertex( m_tess, m_points.back().values,
                     reinterpret_cast<void*>( m_points.size()-1 ) );
   }

   void TerrainManager::
   endUpdate( void )
   {
      gluTessEndPolygon( m_tess );
      processOldTriangles();
      processNewTriangles();
   }

   void TerrainManager::
   tess_begin( GLenum type, TerrainManager* self )
   {
      if( type != GL_TRIANGLES )
         throw std::runtime_error( "GLtessellator must only use GL_TRIANGLES" );
   }

   void TerrainManager::
   tess_end( TerrainManager* self )
   {
   }

   void TerrainManager::
   tess_vertex( std::size_t i, TerrainManager *self )
   {
      if( self->m_tri_index == 0 )
      {
         self->m_new_polygons.push_back( Polygon() );
         self->m_new_polygons.back().num_points = 3;
      }

      Polygon& t = self->m_new_polygons.back();
      t.points[self->m_tri_index] = i;
      ++self->m_tri_index;

      if( self->m_tri_index == 3 )
      {
         self->m_tri_index = 0;
      }
   }

   void TerrainManager::
   tess_edge( GLboolean, TerrainManager* self )
   {
   }

   void TerrainManager::
   tess_combine( GLdouble coords[3], void* vertex_data, GLfloat weight[4],
                 std::size_t * i, TerrainManager* self )
   {
      self->m_points.push_back( Vertex( coords ) );
      *i = self->m_points.size()-1;
   }

   void TerrainManager::tess_error( GLenum gl_errno, TerrainManager *self )
   {
      throw std::runtime_error( reinterpret_cast<const char*>(gluErrorString( gl_errno )) );
   }

   void TerrainManager::
   initAABB( void )
   {
      ento::initAABB( m_aabb );
   }

   void TerrainManager::
   processAABB( void )
   {
      const int32 max_query = 64;
      Polygon* query[max_query];
      int32 count = m_colldet.Query(m_aabb, reinterpret_cast<void**>(query), max_query);
      assert( count < max_query );
      for( int32 i=0; i<count; ++i )
      {
         Polygon& p = *query[i];
         //move triangle into old triangles list
         m_old_polygons.splice( m_old_polygons.end(), m_polygons,
                                 poly2iter( &p ) );

         gluTessBeginContour( m_tess );
         for( unsigned i=0; i<p.num_points; ++i )
         {
            gluTessVertex( m_tess, m_points[p.points[i]].values,
                           reinterpret_cast<void*>( p.points[i] ) );
         }
         gluTessEndContour( m_tess );
      }
   }

/// the inner loop for processOldTriangles
/// @remarks removes 'new' triangles if they match existing triangles
/// @return true if matching triangle found
   bool TerrainManager::
   processOldTriangles_inner( Poly_iter old_iter )
   {
      Poly_iter new_iter = m_new_polygons.begin();
      Poly_iter new_end = m_new_polygons.end();
      while( new_iter != new_end )
      {
         if( false && same( *new_iter, *old_iter ) )
         {
            new_iter = m_new_polygons.erase(new_iter);
            return true;
         }
         ++new_iter;
      }
      return false;
   }

   void TerrainManager::
   processOldTriangles( void )
   {
      Poly_iter old_iter = m_old_polygons.begin();
      Poly_iter old_end = m_old_polygons.end();
      std::size_t old_size = m_old_polygons.size();
      std::cerr << old_size << " to be updated\n";
      while( old_iter != old_end )
      {
         if( processOldTriangles_inner( old_iter ) )
         {
            Poly_iter tmp = old_iter++;
            // move tmp from old tri list to tri list
            m_polygons.splice( m_polygons.end(), m_old_polygons, tmp );
            std::cerr << "1 polygon merge\n";
         }
         else
         {
            if( old_iter->s )
            {
               m_body->DestroyShape( old_iter->s );
            }
            m_colldet.DestroyProxy( old_iter->proxyID );
            m_poly_lookup.erase( m_poly_lookup.find(&(*old_iter)) );
            ++old_iter;
         }
      }
      m_old_polygons.clear();
   }

   void TerrainManager::
   processNewTriangles( void )
   {
      remove_slivers();

      Poly_iter iter = m_new_polygons.begin();
      Poly_iter end = m_new_polygons.end();
      std::cerr << m_new_polygons.size() << " new triangles to be added\n";
      while( iter != end )
      {
         if( ! valid_triangle(*iter) )
         {
            iter = m_new_polygons.erase(iter);
            std::cerr << "throwing away a triangle!\n";
            continue;
         }

         // build up edge info for this polygon
         unsigned last = iter->num_points-1;
         unsigned i = 0;
         while( i < iter->num_points )
         {
            Key k( iter->points[last],iter->points[i]);;
            std::map<Key,EdgeInfo>::iterator found;
            found = m_edge_lookup.find(k);
            // update current edge
            if( found != m_edge_lookup.end() )
            {
               assert( found->second.b == end );
               found->second.b = iter;
            }
            else // create new edge
            {
               EdgeInfo ei;
               ei.a = iter;
               ei.b = end;
               bool success = m_edge_lookup.insert( std::make_pair(k,ei) ).second;
               assert(success);
            }
            last = i++;
         }// for  all points
         ++iter;
      }// while iter is not end

      // ignore all unshared edges
      Edge_iter e_iter = m_edge_lookup.begin();
      Edge_iter e_end = m_edge_lookup.end();
      while( e_iter != e_end )
      {
         if( e_iter->second.a == end ||
             e_iter->second.b == end )
         {
            m_edge_lookup.erase(e_iter++);
         }
         else
         {
            ++e_iter;
         }
      }

      std::cerr << m_edge_lookup.size() << " shared edges\n";

      process_merge();

      std::cerr << m_new_polygons.size() << " new polygons to be added\n";

      // finish all remaining new polygons
      iter = m_new_polygons.begin();
      while( iter != end )
      {
         m_tridef.userData = &(*iter);
         m_tridef.vertexCount = iter->num_points;
         for( int i=0; i<m_tridef.vertexCount; ++i )
         {
            m_tridef.vertices[i] = point2vec( iter->points[i] );
         }
         std::cerr << "creating polygon: " << m_tridef << '\n';
         iter->proxyID = m_colldet.CreateProxy( poly2aabb(*iter), &(*iter) );
         iter->s = static_cast<b2PolygonShape*>( m_body->CreateShape(&m_tridef));
         m_poly_lookup[&(*iter)] = iter;
         ++iter;
      }

      // move entire new list into the real list
      m_polygons.splice( m_polygons.end(), m_new_polygons );
      assert( m_new_polygons.empty() );
   }

   void TerrainManager::
   remove_slivers( void )
   {
      Poly_iter iter = m_new_polygons.begin();
      Poly_iter end = m_new_polygons.end();

      while( iter != end )
      {
         assert( iter->num_points == 3 );

         bool  removed_triangle = false;
         unsigned last = 2;
         for( unsigned i=0; i<3; ++i )
         {
            b2Vec2 edge = point2vec( iter->points[i]) - point2vec( iter->points[last]);
            if( edge.LengthSquared() < 1 )
            {
               removed_triangle = true;
               // keep the lower index, that way we won't invalidate any of the
               //  current polygons, this is the same reason the merge point
               //  is only deleted instead of merged
               std::size_t keep = std::min( iter->points[i], iter->points[last] );
               std::size_t merge = std::max( iter->points[i], iter->points[last] );
               iter = m_new_polygons.erase( iter );
               std::cerr << "removed a sliver " << edge << "\n";

               // update all other triangles
               Poly_iter iter2 = m_new_polygons.begin();
               while( iter2 != end )
               {
                  std::size_t* points = iter2->points;
                  std::size_t* end_ptr = points + iter2->num_points;

                  std::size_t* merge_ptr = std::find( points, end_ptr, merge );
                  if( merge_ptr != end_ptr )
                  {  // remove the ones losing and edge
                     // these will have both the keep and merge ends of the edge
                     if( std::find( points, end_ptr, keep ) != end_ptr )
                     {
                        std::cerr << "chain removing a sliver\n";
                        // in case we're removing iter
                        if( iter == iter2 )
                           { iter = iter2 = m_new_polygons.erase( iter2 ); }
                        else
                           { iter2 = m_new_polygons.erase( iter2 ); }
                     }
                     else
                     { // just updating a point, still has three edges
                        *merge_ptr = keep;
                     }
                  }// if merge present
                  else
                  {
                     ++iter2;
                  }
               }// update all new triangles

               break; // just removed this triangle stop
                      // iterating over its edges
            }// if edge too short

            last = i;
         }// for each edge

         // removing a triangle advances the iterator.
         if( !removed_triangle  )
            { ++iter; }
      }// loop over all new triangles
   }

   /// detect degenerate (flat) triangles
   /// @param p triangle to check ( must be 3 sided poly )
   /// @return whether p is a valid triangle
   bool TerrainManager::
   valid_triangle( const TerrainManager::Polygon& p ) const
   {
      assert( p.num_points == 3 );
      b2Vec2 one;
      b2Vec2 two;
      get_adjacent_edges(p,one,two);
      return b2Cross( two, one) > std::numeric_limits<float>::epsilon();
   }

   /// process each edge info, if the polygons can be merged, send them to do_merge
   void TerrainManager::
   process_merge( void )
   {
      while( !m_edge_lookup.empty() )
      {
         std::map<Key,EdgeInfo>::iterator iter = m_edge_lookup.begin();
         const Key k = iter->first;
         Poly_iter a = iter->second.a;
         Poly_iter b = iter->second.b;

         assert( a != m_new_polygons.end() );
         assert( b != m_new_polygons.end() );

         rotate_to_edge( *a, k );
         b2Vec2 a_ccw;
         b2Vec2 a_cw;
         get_adjacent_edges( *a, a_ccw, a_cw );

         rotate_to_edge( *b, k );
         b2Vec2 b_ccw;
         b2Vec2 b_cw;
         get_adjacent_edges( *b, b_ccw, b_cw );

         m_edge_lookup.erase(iter);

         const float epsilon = std::numeric_limits<float>::epsilon();

         if( b2Cross( a_ccw, b_cw ) > epsilon &&
             b2Cross( b_ccw, a_cw ) > epsilon )
         {
            do_merge( a, b );
            m_new_polygons.erase( b );
         }
      }// while
   }

   /// rotate polygons points so that the start and end point are the shared edge
   /// @param p polygon to rotate
   /// @param k key of edge to rotate to
   void TerrainManager::
   rotate_to_edge( TerrainManager::Polygon& p, const TerrainManager::Key& k ) const
   {
      // find index of end of edge
      std::size_t index;
      if( p.points[0] == k.lo || p.points[0] == k.hi )
      {
         if( p.points[p.num_points-1] == k.lo || p.points[p.num_points-1] == k.hi )
         {
            index = 0;
         }
         else
         {
            assert( p.points[1] == k.lo || p.points[1] == k.hi );
            index = 1;
         }
      }
      else
      {
         for( unsigned i=1; i<p.num_points; ++i )
         {
            if( p.points[i] == k.lo || p.points[i] == k.hi )
            {
               assert( p.points[i+1] == k.lo || p.points[i+1] == k.hi );
               index = i + 1;
               break;
            }
         }
      }

      std::rotate( p.points, p.points+index, p.points+p.num_points );
   }

   /// get the normalized edge leading away from the shared edge
   /// @param primary will contain CCW edge
   /// @param secondary will contain CW edge
   void TerrainManager::
   get_adjacent_edges( const TerrainManager::Polygon& p,
                       b2Vec2& ccw, b2Vec2& cw ) const
   {
      assert( p.num_points > 2 );
      ccw = point2vec(p.points[1]) - point2vec(p.points[0]);
      ccw *= b2InvSqrt(ccw.LengthSquared());
      cw = point2vec(p.points[p.num_points - 2]) -
                  point2vec(p.points[p.num_points - 1]);
      cw *= b2InvSqrt(cw.LengthSquared());
   }

   /// merge b into a
   /// @param a polygon sharing an edge
   /// @param b polygon sharing an edge
   void TerrainManager::
   do_merge( TerrainManager::Poly_iter a, TerrainManager::Poly_iter b )
   {
      assert( a->points[0] == b->points[ b->num_points-1] );
      assert( b->points[0] == a->points[ a->num_points-1] );

      // skip the edge points (the first and last point)
      for( unsigned i=1; i<b->num_points-1; ++i )
      {
         a->points[ a->num_points++ ] = b->points[i];
      }

      // update edge list so that edges that used to be shared with b
      // are now shared with a
      std::map<Key,EdgeInfo>::iterator iter, end;
      iter = m_edge_lookup.begin();
      end = m_edge_lookup.end();
      while( iter != end )
      {
         if( b == iter->second.a )
         {
            if( iter->second.b->num_points + a->num_points > b2_maxPolygonVertices + 2 )
            {
               // the combined polygon would have too many vertices, so just
               // discard it
               m_edge_lookup.erase( iter++ );
            }
            else
            {
               iter->second.a = a;
               ++iter;
            }
         }
         else if( b == iter->second.b )
         {
            if( iter->second.a->num_points + a->num_points > b2_maxPolygonVertices + 2 )
            {
               m_edge_lookup.erase( iter++ );
            }
            else
            {
               iter->second.b = a;
               ++iter;
            }
         }
         else
         {
            ++iter;
         }
      }

   }

   b2Vec2 TerrainManager::
   point2vec( std::size_t point ) const
   {
      return b2Vec2( m_points[point].values[0], m_points[point].values[1] );
   }

   TerrainManager::Poly_iter TerrainManager::
   poly2iter( Polygon* poly ) const
   {
      std::map<Polygon*,Poly_iter>::const_iterator found;
      found = m_poly_lookup.find(poly);
      if( found == m_poly_lookup.end() )
      {
         throw std::runtime_error("Triangle lookup failed");
      }
      return found->second;
   }

   b2AABB TerrainManager::
   poly2aabb( const TerrainManager::Polygon& poly ) const
   {
      b2AABB aabb;
      ento::initAABB(aabb);

      for( unsigned i=0; i<poly.num_points; ++i )
      {
         ento::expandAABB( aabb, point2vec(poly.points[i]) );
      }

      return aabb;
   }

   /// @return whether lhs is the same triangle as rhs
   /// @param lhs
   /// @param rhs
   bool TerrainManager::
   same ( const TerrainManager::Polygon& lhs,
          const TerrainManager::Polygon& rhs ) const
   {
      if( rhs.points[0] == lhs.points[0] &&
          rhs.points[1] == lhs.points[1] &&
          rhs.points[2] == lhs.points[2] )
      {
         return true;
      }
      else if( rhs.points[1] == lhs.points[0] &&
               rhs.points[2] == lhs.points[1] &&
               rhs.points[0] == lhs.points[2] )
      {
         return true;
      }
      else if( rhs.points[2] == lhs.points[0] &&
               rhs.points[0] == lhs.points[1] &&
               rhs.points[1] == lhs.points[2] )
      {
         return true;
      }
      return false;
   }

//
// --- TerrainManager::Vertex ---
//
   TerrainManager::Vertex::Vertex( GLdouble* coords )
   {
      values[0] = coords[0];
      values[1] = coords[1];
      values[2] = 0.0;
   }

   TerrainManager::Vertex::Vertex( const b2Vec2& vec )
   {
      values[0] = vec.x;
      values[1] = vec.y;
      values[2] = 0.0;
   }
}