#define png_infopp_NULL (png_infopp) NULL
#define int_p_NULL (int*)NULL

#include <math.h>
#include <array>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <boost/gil/gil_all.hpp>
#include <boost/shared_ptr.hpp>
#include <fstream>
#include <ignition/math/Vector3.hh>
#include <iostream>
#include <sdf/sdf.hh>

#include "collision_map_request.pb.h"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"

namespace gazebo
{
typedef const boost::shared_ptr<const collision_map_creator_msgs::msgs::CollisionMapRequest> CollisionMapRequestPtr;

class CollisionMapCreator : public WorldPlugin
{
  transport::NodePtr node;
  transport::PublisherPtr imagePub;
  transport::SubscriberPtr commandSubscriber;
  physics::WorldPtr world;

public:
  void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
  {
    node = transport::NodePtr(new transport::Node());
    world = _parent;
    // Initialize the node with the world name
    node->Init(world->Name());
    std::cout << "Subscribing to: "
              << "~/collision_map/command" << std::endl;
    commandSubscriber = node->Subscribe("~/collision_map/command", &CollisionMapCreator::create, this);
    imagePub = node->Advertise<msgs::Image>("~/collision_map/image");
  }

public:
  void create(CollisionMapRequestPtr& msg)
  {
    std::cout << "Received message" << std::endl;

    std::cout << "Creating collision map with corners at (" << msg->upperleft().x() << ", " << msg->upperleft().y()
              << "), (" << msg->upperright().x() << ", " << msg->upperright().y() << "), (" << msg->lowerright().x()
              << ", " << msg->lowerright().y() << "), (" << msg->lowerleft().x() << ", " << msg->lowerleft().y()
              << ") with collision projected from z = (" << msg->minheight() << ", " << msg->maxheight()
              << ") \nResolution = " << msg->resolution() << " m\n"
              << "Occupied spaces will be filled with: " << msg->threshold() << std::endl;

    double dX_vertical = msg->upperleft().x() - msg->lowerleft().x();
    double dY_vertical = msg->upperleft().y() - msg->lowerleft().y();
    double mag_vertical = sqrt(dX_vertical * dX_vertical + dY_vertical * dY_vertical);
    dX_vertical = msg->resolution() * dX_vertical / mag_vertical;
    dY_vertical = msg->resolution() * dY_vertical / mag_vertical;

    double dX_horizontal = msg->upperright().x() - msg->upperleft().x();
    double dY_horizontal = msg->upperright().y() - msg->upperleft().y();
    double mag_horizontal = sqrt(dX_horizontal * dX_horizontal + dY_horizontal * dY_horizontal);
    dX_horizontal = msg->resolution() * dX_horizontal / mag_horizontal;
    dY_horizontal = msg->resolution() * dY_horizontal / mag_horizontal;

    int count_vertical = mag_vertical / msg->resolution();
    int count_horizontal = mag_horizontal / msg->resolution();

    if (count_vertical == 0 || count_horizontal == 0)
    {
      std::cout << "Image has a zero dimensions, check coordinates" << std::endl;
      return;
    }
    double x, y, z;
    const double z_min = msg->minheight();
    const double z_max = msg->maxheight();
    const double z_mean = 0.5 * (z_min + z_max);
    boost::gil::gray8_pixel_t fill(255 - msg->threshold());
    boost::gil::gray8_pixel_t blank(255);
    boost::gil::gray8_image_t image(count_horizontal, count_vertical);

    ignition::math::Vector3d start, end;

#if GAZEBO_MAJOR_VERSION >= 8
    gazebo::physics::PhysicsEnginePtr engine = world->Physics();
#else
    gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
#endif
    engine->InitForThread();
    gazebo::physics::RayShapePtr ray = boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
        engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

    std::cout << "Rasterizing model and checking collisions" << std::endl;
    boost::gil::fill_pixels(image._view, blank);

    const double dZ = std::abs(z_max - z_min) / 2.;
    const double dX = (dX_vertical + dX_horizontal) / 2.;
    const double dY = (dY_vertical + dY_horizontal) / 2.;

    typedef std::array<double, 6> StartEndPoints;
    std::list<StartEndPoints> deltas;  // 3 double for x,y,z start 3 for x,y,z end

    deltas.emplace_back(StartEndPoints{ -dX, 0., 0, dX, 0., 0. });
    deltas.emplace_back(StartEndPoints{ 0., -dY, 0, 0., dY, 0. });
    deltas.emplace_back(StartEndPoints{ 0., 0., -dZ, 0., 0., dZ });

    double dist;
    std::string entityName;

    for (int i = 0; i < count_vertical; ++i)
    {
      std::cout << "Percent complete: " << std::setw(6) << std::setfill(' ')
                << static_cast<double>(i) * 100.0 / static_cast<double>(count_vertical) << " %" << std::endl;
      x = i * dX_vertical +
          msg->lowerleft().x();  // TODO: check if the amcl need this --> + dX_vertical / 2. + dX_horizontal / 2.;
      y = i * dY_vertical +
          msg->lowerleft().y();  // TODO: check if the amcl need this --> dY_vertical / 2. + dY_horizontal / 2.;
      z = z_mean;
      for (int j = 0; j < count_horizontal; ++j)
      {
        x += dX_horizontal;
        y += dY_horizontal;

        for (const auto& delta : deltas)
        {
          const double x_start = x + delta[0];
          const double y_start = y + delta[1];
          const double z_start = z + delta[2];

          start.X(x_start);
          start.Y(y_start);
          start.Z(z_start);

          const double x_end = x + delta[3];
          const double y_end = y + delta[4];
          const double z_end = z + delta[5];
          end.X(x_end);
          end.Y(y_end);
          end.Z(z_end);

          ray->SetPoints(start, end);
          ray->GetIntersection(dist, entityName);

          if (!entityName.empty())
          {
            image._view(i, j) = fill;
            break;
          }
        }
      }
    }

    std::cout << "Completed calculations, writing to image" << std::endl;
    if (!msg->filename().empty())
    {
      boost::gil::gray8_view_t view = image._view;

      // Write to png
      // boost::gil::png_write_view(msg->filename(), view);
      // Write to pgm (pnm p2)
      pgm_write_view(msg->filename(), view);

      yaml_write(msg->filename(), msg->lowerleft().x(), msg->lowerleft().y(), z_mean, msg->resolution());
    }
    std::cout << "Output location: " << msg->filename() << std::endl;
  }

public:
  void pgm_write_view(const std::string& filename, boost::gil::gray8_view_t& view)
  {
    // Write image to pgm file
    std::cout << "running" << std::endl;
    int h = view.height();
    int w = view.width();

    std::ofstream ofs;
    ofs.open(filename + ".pgm");
    ofs << "P2" << '\n';           // grayscale
    ofs << w << ' ' << h << '\n';  // width and height
    ofs << 255 << '\n';            // max value
    for (int y = 0; y < h; ++y)
    {
      for (int x = 0; x < w; ++x)
      {
        // std::cout << (int)view(x, y)[0];
        ofs << (int)view(x, y)[0] << ' ';
      }
      ofs << '\n';
    }
    ofs.close();
  }

  void yaml_write(const std::string& filename, const double& x_orig, const double& y_orig, const double z_orig,
                  const double& resolution)
  {
    auto pos = filename.rfind("/");
    std::string map_name = ".pgm";

    if (pos != std::string::npos)
    {
      map_name = filename.substr(pos + 1) + map_name;
    }
    else
    {
      map_name = filename + map_name;
    }

    std::ofstream ofs;
    ofs.open(filename + ".yaml");

    ofs << "image: " << map_name << std::endl;
    ofs << "resolution: " << resolution << std::endl;
    ofs << "origin: [" << x_orig << ", " << y_orig << ", 0 ]" << std::endl;
    ofs << "occupied_thresh: 0.65" << std::endl;
    ofs << "free_thresh: 0.35" << std::endl;
    ofs << "negate: 0" << std::endl;
    ofs.close();
  }
};

// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(CollisionMapCreator)
}  // namespace gazebo
