#ifndef _LOCAL_MAP_
#define _LOCAL_MAP_
//TO DO LIST map esdf  astar backend

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <map>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace local_map_util{
    using Tmap = std::vector<signed char>;
    class MapUtil {
    public:
      ///Simple constructor
      MapUtil() {}
      ///Get map data
      Tmap getMap() { return map_; }
      //
      bool has_map_(){return has_map;}
      ///Get resolution
      double getRes() { return res_; }
      ///Get dimensions
      Eigen::Vector3i getDim() { return dim_; }
      ///Get origin
      Eigen::Vector3d getOrigin() { return origin_d_; }
      Eigen::Vector3d getMapSize() { return map_size; }
      void setParam(ros::NodeHandle& nh){
        nh.param("grid_map/map_size_x", map_size(0), 60.0);
        nh.param("grid_map/map_size_y", map_size(1), 60.0);
        nh.param("grid_map/map_size_z", map_size(2), 2.0);
        nh.param("grid_map/resolution", res_, 0.1);
        nh.param("grid_map/local_update_range_x", update_range_(0), -1.0);
        nh.param("grid_map/local_update_range_y", update_range_(1), -1.0);
        nh.param("grid_map/local_update_range_z", update_range_(2), -1.0);
        expand_size = 0;
        int buffer_size;
        origin_d_[0] = -map_size(0)/2;
        origin_d_[1] = -map_size(1)/2;
        origin_d_[2] = -2.0;
        dim_(0) = map_size(0)/res_;
        dim_(1) = map_size(1)/res_;
        dim_(2) = map_size(2)/res_;
        buffer_size = dim_(0)*dim_(1)*dim_(2);
        map_.resize(buffer_size,val_free);
        // global_cloud_vis
        //esdf
        distance_buffer_ = std::vector<double>(buffer_size, 10000);
        distance_buffer_neg_ = std::vector<double>(buffer_size, 10000);
        distance_buffer_all_ = std::vector<double>(buffer_size, 10000);
        tmp_buffer1_ = std::vector<double>(buffer_size, 0);
        tmp_buffer2_ = std::vector<double>(buffer_size, 0);
        occupancy_buffer_neg = std::vector<char>(buffer_size, 0);
        esdf_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/local_esdf", 10);
        pcl_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/local_pcl", 10);

        local_bound_min_ = Eigen::Vector3i::Zero();
        local_bound_max_ = dim_ - Eigen::Vector3i::Ones();
        update_range_grid_[0] = int(update_range_[0] / res_);
        update_range_grid_[1] = int(update_range_[1] / res_);
        update_range_grid_[2] = int(update_range_[2] / res_);

      }





      void SetMapBuild(const sensor_msgs::PointCloud2 & pointcloud_map, Eigen::Vector3d curpos){
        //reset buffer
        double t0 = ros::Time::now().toSec();
        Eigen::Vector3i curidx = floatToInt(curpos);
        local_bound_min_ = curidx - update_range_grid_;
        local_bound_max_ = curidx + update_range_grid_;
        local_bound_min_[0] = max(local_bound_min_[0], 0);
        local_bound_min_[1] = max(local_bound_min_[1], 0);
        local_bound_min_[2] = max(local_bound_min_[2], 0);
        local_bound_max_[0] = min(local_bound_max_[0], dim_(0)-1);
        local_bound_max_[1] = min(local_bound_max_[1], dim_(1)-1);
        local_bound_max_[2] = min(local_bound_max_[2], dim_(2)-1);


        /* reset occ and dist buffer */
        for (int x = local_bound_min_(0); x <= local_bound_max_(0); ++x)
          for (int y = local_bound_min_(1); y <= local_bound_max_(1); ++y)
            for (int z = local_bound_min_(2); z <= local_bound_max_(2); ++z) {
              map_[getIndex(Eigen::Vector3i(x, y, z))] = val_free;
              distance_buffer_[getIndex(Eigen::Vector3i(x, y, z))] = 10000;
            }


        double t1 = ros::Time::now().toSec();

        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(pointcloud_map, cloud);
        if( (int)cloud.points.size() == 0 ) {
          return;
        }
        pcl::PointXYZ pt;
        for (int idx = 0; idx < (int)cloud.points.size(); idx++)
        {
          pt = cloud.points[idx];
          Eigen::Vector3i idx3 = floatToInt(Eigen::Vector3d(pt.x, pt.y, pt.z));
          if(idx3[0] >= local_bound_min_[0] && idx3[1] >= local_bound_min_[1] && idx3[2]>=local_bound_min_[2]&&
             idx3[0] < local_bound_max_[0] &&  idx3[1] < local_bound_max_[1] &&  idx3[2] < local_bound_max_[2]
           )
           map_[getIndex(idx3)] = val_occ;
        }

        has_map = true;
        double t2 = ros::Time::now().toSec();
        updateESDF3d();
        double t3 = ros::Time::now().toSec();
        // publishESDF();
        return;
      }
   
      ///Get index of a cell id(1) * global_map_size_(0) + id(0)
      int getIndex(const Eigen::Vector3i& pn) {
          return pn(0) + dim_(0) * pn(1) + dim_(0) * dim_(1) * pn(2);
      }
      ///Check if the given cell is outside of the map in i-the dimension
      bool isOutsideXYZ(const Eigen::Vector3i &n, int i) { return n(i) < 0 || n(i) >= dim_(i); }
      ///Check if the cell is free by index
      bool isFree(int idx) { return map_[idx] == val_free; }
      ///Check if the cell is unknown by index
      ///Check if the cell is occupied by index
      bool isOccupied(int idx) { return map_[idx] > val_free; }
      //free 0 occ 100 unknow -1
      ///Check if the cell is outside by coordinate
      bool isOutside(const Eigen::Vector3i &pn) {
        for(int i = 0; i < 3; i++)
          if (pn(i) < 0 || pn(i) >= dim_(i)){
            // std::cout <<"i: "<<i<<"cell is outside by coordinate: "<<pn(i)<<" dim_(i)"<<dim_(i)<<std::endl;
            return true;
          }
        return false;
      }
      ///Check if the given cell is free by coordinate
      bool isFree(const Eigen::Vector3i &pn) {
        if (isOutside(pn))
          return false;
        else
          return isFree(getIndex(pn));
      }
      ///Check if the given cell is occupied by coordinate
      bool isOccupied(const Eigen::Vector3i &pn) {
        if (isOutside(pn))
          return true;
        else
          return isOccupied(getIndex(pn));
      }
      bool isOccupied(const Eigen::Vector3d &pt) {
        Eigen::Vector3i pn = floatToInt(pt);
        if (isOutside(pn))
          return true;
        else
          return isOccupied(getIndex(pn));
      }
      



      ///Print basic information about the util
      void info() {
        Eigen::Vector3d range = dim_.template cast<double>() * res_;
        std::cout << "MapUtil Info ========================== " << std::endl;
        std::cout << "   res: [" << res_ << "]" << std::endl;
        std::cout << "   origin: [" << origin_d_.transpose() << "]" << std::endl;
        std::cout << "   range: [" << range.transpose() << "]" << std::endl;
        std::cout << "   dim: [" << dim_.transpose() << "]" << std::endl;
      };

      ///Float position to discrete cell coordinate
      Eigen::Vector3i floatToInt(const Eigen::Vector3d &pt) {
        Eigen::Vector3i pn;
        for(int i = 0; i < 3; i++)
          pn(i) = floor((pt(i) - origin_d_(i)) / res_);
        return pn;
      }
      ///Discrete cell coordinate to float position
      Eigen::Vector3d intToFloat(const Eigen::Vector3i &pn) {
        //return pn.template cast<double>() * res_ + origin_d_;
        return (pn.template cast<double>() + Eigen::Vector3d::Constant(0.5)) * res_ + origin_d_;
      }

     

      ///Get occupied voxels
      std::vector<Eigen::Vector3d> getCloud() {
        std::vector<Eigen::Vector3d> cloud;
        Eigen::Vector3i n;
        for (n(0) = 0; n(0) < dim_(0); n(0)++) {
          for (n(1) = 0; n(1) < dim_(1); n(1)++) {
            for (n(2) = 0; n(2) < dim_(2); n(2)++) {
              if (isOccupied(getIndex(n)))
                cloud.push_back(intToFloat(n));
            }
          }
        }

        return cloud;
      }
      void updateESDF3d() {
        Eigen::Vector3i min_esdf = local_bound_min_;
        Eigen::Vector3i max_esdf = local_bound_max_;

        /* ========== compute positive DT ========== */

        for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
          for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
            fillESDF(
                [&](int z) {
                  return isOccupied(Eigen::Vector3i(x,y,z))?
                      0 :
                      std::numeric_limits<double>::max();
                },
                [&](int z, double val) { tmp_buffer1_[getIndex(Eigen::Vector3i(x,y,z))] = val; }, min_esdf[2],
                max_esdf[2], 2);
          }
        }

        for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
          for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
            fillESDF([&](int y) { return tmp_buffer1_[getIndex(Eigen::Vector3i(x,y,z))]; },
                    [&](int y, double val) { tmp_buffer2_[getIndex(Eigen::Vector3i(x,y,z))] = val; }, min_esdf[1],
                    max_esdf[1], 1);
          }
        }

        for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
          for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
            fillESDF([&](int x) { return tmp_buffer2_[getIndex(Eigen::Vector3i(x,y,z))]; },
                    [&](int x, double val) {
                      distance_buffer_[getIndex(Eigen::Vector3i(x,y,z))] = res_ * std::sqrt(val);
                      //  min(mp_.res_ * std::sqrt(val),
                      //      md_.distance_buffer_[toAddress(x, y, z)]);
                    },
                    min_esdf[0], max_esdf[0], 0);
          }
        }

        /* ========== compute negative distance ========== */
        for (int x = min_esdf(0); x <= max_esdf(0); ++x)
          for (int y = min_esdf(1); y <= max_esdf(1); ++y)
            for (int z = min_esdf(2); z <= max_esdf(2); ++z) {

              int idx = getIndex(Eigen::Vector3i(x,y,z));
              if (!isOccupied(Eigen::Vector3i(x,y,z))) {
                occupancy_buffer_neg[idx] = 1;

              } else if (isOccupied(Eigen::Vector3i(x,y,z))) {
                occupancy_buffer_neg[idx] = 0;
              } else {
                ROS_ERROR("what?");
              }
            }

        ros::Time t1, t2;

        for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
          for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
            fillESDF(
                [&](int z) {
                  return occupancy_buffer_neg[getIndex(Eigen::Vector3i(x,y,z))] == 1 ?
                      0 :
                      std::numeric_limits<double>::max();
                },
                [&](int z, double val) { tmp_buffer1_[getIndex(Eigen::Vector3i(x,y,z))] = val; }, min_esdf[2],
                max_esdf[2], 2);
          }
        }

        for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
          for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
            fillESDF([&](int y) { return tmp_buffer1_[getIndex(Eigen::Vector3i(x,y,z))]; },
                    [&](int y, double val) { tmp_buffer2_[getIndex(Eigen::Vector3i(x,y,z))] = val; }, min_esdf[1],
                    max_esdf[1], 1);
          }
        }

        for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
          for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
            fillESDF([&](int x) { return tmp_buffer2_[getIndex(Eigen::Vector3i(x,y,z))]; },
                    [&](int x, double val) {
                      distance_buffer_neg_[getIndex(Eigen::Vector3i(x,y,z))] = res_ * std::sqrt(val);
                    },
                    min_esdf[0], max_esdf[0], 0);
          }
        }

        /* ========== combine pos and neg DT ========== */
        for (int x = min_esdf(0); x <= max_esdf(0); ++x)
          for (int y = min_esdf(1); y <= max_esdf(1); ++y)
            for (int z = min_esdf(2); z <= max_esdf(2); ++z) {

              int idx = getIndex(Eigen::Vector3i(x,y,z));
              distance_buffer_all_[idx] = distance_buffer_[idx];

              if (distance_buffer_neg_[idx] > 0.0)
                distance_buffer_all_[idx] += (-distance_buffer_neg_[idx] + res_);
            }
      }


      template <typename F_get_val, typename F_set_val>
      void fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int d) {
        int v[dim_(d)];
        double z[dim_(d) + 1];

        int k = start;
        v[start] = start;
        z[start] = -std::numeric_limits<double>::max();
        z[start + 1] = std::numeric_limits<double>::max();

        for (int q = start + 1; q <= end; q++) {
          k++;
          double s;

          do {
            k--;
            s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]);
          } while (s <= z[k]);

          k++;

          v[k] = q;
          z[k] = s;
          z[k + 1] = std::numeric_limits<double>::max();
        }

        k = start;

        for (int q = start; q <= end; q++) {
          while (z[k + 1] < q) k++;
          double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
          f_set_val(q, val);
        }
      }
      inline double getDistance(const Eigen::Vector3d& pos) {
        if(isOutside(floatToInt(pos))){
          return 1.0;
        }

        Eigen::Vector3i id;
        id = floatToInt(pos);
        boundIndex(id);
        return distance_buffer_all_[getIndex(id)];
      }

      inline double getDistance(const Eigen::Vector3i& id) {
        if(isOutside(id)){
          return 1.0;
        }
        Eigen::Vector3i id1 = id;
        boundIndex(id1);
        return distance_buffer_all_[getIndex(id1)];
      }

      inline double* getEsdfData(){
        return distance_buffer_all_.data();
      }

      inline void boundIndex(Eigen::Vector3i& id) {
        Eigen::Vector3i id1;
        id1(0) = std::max(std::min(id(0), dim_(0) - 1), 0);
        id1(1) = std::max(std::min(id(1), dim_(1) - 1), 0);
        id1(2) = std::max(std::min(id(2), dim_(2) - 1), 0);
        id = id1;
    }
    void publishPCL(){
      std::vector<Eigen::Vector3d> pcs = getCloud();
      pcl::PointCloud<pcl::PointXYZI> cloud;
      pcl::PointXYZI pt;
      ROS_INFO_STREAM("publish: "<<pcs.size());
      for(const auto& pc : pcs){
        pt.x = pc[0];
        pt.y = pc[1];
        pt.z = pc[2];
        cloud.push_back(pt);
      }
      cloud.width = cloud.points.size();
      cloud.height = 1;
      cloud.is_dense = true;
      cloud.header.frame_id = "world";
      sensor_msgs::PointCloud2 cloud_msg;
      pcl::toROSMsg(cloud, cloud_msg);
      pcl_pub_.publish(cloud_msg);

    }
    void publishESDF() {
      double dist;
      pcl::PointCloud<pcl::PointXYZI> cloud;
      pcl::PointXYZI pt;
      const double min_dist = -4.0;
      const double max_dist = 4.0;
      Eigen::Vector3i min_cut = Eigen::Vector3i(0,0,0);
      Eigen::Vector3i max_cut = Eigen::Vector3i(dim_(0)-1,dim_(1)-1,dim_(2)-1);

      for (int x = min_cut(0); x <= max_cut(0); ++x)
        for (int y = min_cut(1); y <= max_cut(1); ++y) 
          {
          Eigen::Vector3d pos;
          pos = intToFloat(Eigen::Vector3i(x, y, 10));
          dist = getDistance(pos);
          pt.x = pos(0);
          pt.y = pos(1);
          pt.z = pos(2);
          pt.intensity = (dist - min_dist) / (max_dist - min_dist);
          // std::cout << "pos: " << pos.transpose() << std::endl;
          cloud.push_back(pt);
        }
      cloud.width = cloud.points.size();
      cloud.height = 1;
      cloud.is_dense = true;
      cloud.header.frame_id = "world";
      sensor_msgs::PointCloud2 cloud_msg;
      pcl::toROSMsg(cloud, cloud_msg);
      esdf_pub_.publish(cloud_msg);
    }
    inline double getDistGrad(Eigen::Vector3d po, Eigen::Vector3d& grad) {
        Eigen::Vector3d pos(po(0),po(1),po(2));
        if (isOutside(floatToInt(pos))) {
          grad.setZero();
          return 0;
        }
        /* use trilinear interpolation */
        Eigen::Vector3d pos_m = pos - 0.5 * res_ * Eigen::Vector3d::Ones();

        Eigen::Vector3i idx= floatToInt(pos_m);

        Eigen::Vector3d idx_pos, diff;
        idx_pos = intToFloat(idx);

        diff = (pos - idx_pos) / res_;

        double values[2][2][2];
        for (int x = 0; x < 2; x++) {
          for (int y = 0; y < 2; y++) {
            for (int z = 0; z < 2; z++) {
              Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z);
              values[x][y][z] = getDistance(current_idx);
            }
          }
        }

        double v00 = (1 - diff[0]) * values[0][0][0] + diff[0] * values[1][0][0];
        double v01 = (1 - diff[0]) * values[0][0][1] + diff[0] * values[1][0][1];
        double v10 = (1 - diff[0]) * values[0][1][0] + diff[0] * values[1][1][0];
        double v11 = (1 - diff[0]) * values[0][1][1] + diff[0] * values[1][1][1];
        double v0 = (1 - diff[1]) * v00 + diff[1] * v10;
        double v1 = (1 - diff[1]) * v01 + diff[1] * v11;
        double dist = (1 - diff[2]) * v0 + diff[2] * v1;

        grad[2] = (v1 - v0) / res_;
        grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01))  / res_;
        grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
        grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
        grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
        grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);

        grad[0] /= res_;

        return dist;
      }
      
      ///Get free voxels
      std::vector<Eigen::Vector3d> getFreeCloud() {
        std::vector<Eigen::Vector3d> cloud;
        Eigen::Vector3i n;
        for (n(0) = 0; n(0) < dim_(0); n(0)++) {
          for (n(1) = 0; n(1) < dim_(1); n(1)++) {
            for (n(2) = 0; n(2) < dim_(2); n(2)++) {
              if (isFree(getIndex(n)))
                cloud.push_back(intToFloat(n));
            }
          }
        }

        return cloud;
      }

     
      ///Map entity
      Tmap map_;
      std::vector<double> distance_buffer_;
      std::vector<double> distance_buffer_neg_;
      std::vector<char> occupancy_buffer_neg;
      std::vector<double> distance_buffer_all_;
      std::vector<double> tmp_buffer1_;
      std::vector<double> tmp_buffer2_;
      std::string world_frame_id;
      ros::Timer pubTimer;
    protected:
      ///Resolution
      double res_;
      ///Origin, float type
      Eigen::Vector3d origin_d_;
      ///Dimension, int type
      Eigen::Vector3i dim_;
      Eigen::Vector3d map_size;
      Eigen::Vector3d update_range_;
      Eigen::Vector3i local_bound_min_, local_bound_max_, update_range_grid_;
      ///Assume occupied cell has value 100
      int8_t val_occ = 100;
      ///Assume free cell has value 0
      int8_t val_free = 0;
      ///Assume unknown cell has value -1
      int8_t val_unknown = -1;
      ///inflate size
      int  expand_size = 0;
      bool has_map = false;
      ros::Subscriber point_cloud_sub_;
      ros::Publisher esdf_pub_;
      ros::Publisher pcl_pub_;
  };
  typedef MapUtil VoxelMapUtil;
}

#endif
