#include "caching-undistort-inverse.hpp"

#include "perceive/graphics/bresenham.hpp"
#include "perceive/io/fp-io.hpp"
#include "perceive/io/ieee754-packing.hpp"
#include "perceive/io/json-io.hpp"
#include "perceive/io/perceive-assets.hpp"
#include "perceive/io/xdr.hpp"
#include "perceive/utils/create-cv-remap.hpp"
#include "perceive/utils/file-system.hpp"
#include "perceive/utils/md5.hpp"

#include <boost/endian/conversion.hpp>

#define This CachingUndistortInverse

namespace perceive
{
static void get_cached_mapxy(const unsigned w,
                             const unsigned h,
                             const Matrix3r& H_in,
                             const CachingUndistortInverse& cu,
                             const Matrix3r& H_out,
                             cv::Mat& mapx,
                             cv::Mat& mapy,
                             const bool feedback = false);

// -----------------------------------------------------------------------------
// --                        CachingUndistortInverse                          --
// -----------------------------------------------------------------------------

namespace detail
{
   struct CachingUndistortInverseStore
   {
    private:
      mutable std::mutex padlock_;
      std::unordered_map<string, CachingUndistortInverse> lookup_;

    public:
      bool store_cu(const string& key,
                    const CachingUndistortInverse& cu) noexcept
      {
         lock_guard<decltype(padlock_)> lock(padlock_);
         lookup_.insert_or_assign(key, cu);
         return true;
      }

      bool load_cu(const string& key,
                   CachingUndistortInverse& cu) const noexcept
      {
         lock_guard<decltype(padlock_)> lock(padlock_);
         const auto ii      = lookup_.find(key);
         const auto success = ii != cend(lookup_);
         if(success) cu = ii->second;
         return success;
      }
   };

   static CachingUndistortInverseStore& cu_store() noexcept
   {
      static CachingUndistortInverseStore store_;
      return store_;
   }
} // namespace detail

// --------------------------------------------------------------------- ApproxK
//
Vector2 This::ApproxK::distort(const Vector2& U) const noexcept
{
   return homgen_P2_to_R2(to_vec3(K_inv_ * Vector3r(U.x, U.y, 1.0)));
}

Vector2 This::ApproxK::undistort(const Vector2& D) const noexcept
{
   return homgen_P2_to_R2(to_vec3(K_ * Vector3r(D.x, D.y, 1.0)));
}

const This::ApproxK* This::nearest_to_undistorted(const Vector2& U) const
{
   auto ii = rng::smallest_elem(
       approx_Ks_, std::less<real>(), [&U](const auto& ak) -> real {
          return (U - ak.Uc_).quadrance();
       });
   return (ii == cend(approx_Ks_)) ? nullptr : &(*ii);
}

const This::ApproxK* This::nearest_to_distorted(const Vector2& D) const
{
   auto ii = rng::smallest_elem(
       approx_Ks_, std::less<real>(), [&D](const auto& ak) {
          return (D - ak.Dc_).quadrance();
       });
   return (ii == cend(approx_Ks_)) ? nullptr : &(*ii);
}

Vector2 This::approx_distort(const Vector2& U) const noexcept
{
   const auto ak_ptr = nearest_to_undistorted(U);
   return (ak_ptr == nullptr) ? Vector2::nan() : ak_ptr->distort(U);
}

Vector2 This::approx_undistort(const Vector2& D) const noexcept
{
   const auto ak_ptr = nearest_to_distorted(D);
   return (ak_ptr == nullptr) ? Vector2::nan() : ak_ptr->undistort(D);
}

// -------------------------------------------------------------- init-approx-Ks
//
void This::init_approx_Ks_()
{
   if(is_K_) {
      // No need to create approx_Ks, when we're already just using 'K'
      approx_Ks_.clear();
      return;
   }

   const auto& M    = M_;
   const auto& hull = M.calib_hull();
   const auto w     = M.calib_format().x;
   const auto h     = M.calib_format().y;
   const auto C     = Vector2(0.5 * w, 0.5 * h);

   array<Vector3, 4> lls; // lines that we intersect the hull with.
   lls[0] = to_homgen_line(C, C + Vector2(1.0, 0.0));  // horizontal
   lls[1] = to_homgen_line(C, C + Vector2(0.0, 1.0));  // vertical
   lls[2] = to_homgen_line(C, C + Vector2(1.0, -1.0)); // diagonal
   lls[3] = to_homgen_line(C, C + Vector2(-1.0, 1.0)); // diagonal

   // Find all the points of intersection between the hull and these 4 lines.
   // *SHOULD* be 8 points
   vector<Vector2> isects;
   isects.reserve(8);
   for(const auto& ll : lls) {
      vector<Vector2> tmp;
      tmp.reserve(2);
      hull_line_intersections(cbegin(hull), cend(hull), ll, tmp);
      isects.insert(end(isects), cbegin(tmp), cend(tmp));
   }

   // Initialize our result object
   auto init_aK = [&](const Vector2& D0) {
      const auto CD = (D0 - C);      // vector from C -> D0
      const auto D  = C + 0.80 * CD; // move 'D0' towards the center

      ApproxK ak;
      ak.Dc_ = D;
      ak.Uc_ = undistort(D);

      Matrix3r K = Matrix3r::Identity();
      // Such that Us[i] = K * Ds[i]
      // TODO, this has to be completed
      // calibration::estimate_homography_LLS(Ds, Us, K);
      ak.K_     = K;
      ak.K_inv_ = K.inverse();

      return ak;
   };

   approx_Ks_.resize(isects.size());
   std::transform(cbegin(isects), cend(isects), begin(approx_Ks_), init_aK);
}

// ------------------------------------------------------------------------ init
//
void This::init(AABB aabb, const DistortionModel& M, EmpiricalFieldF&& field)
{
   M_ = M;
   M_.finalize();
   is_K_     = false;
   K_        = Matrix3r::Identity();
   K_inv_    = K_.inverse();
   aabb_     = aabb;
   A_str_    = M.to_json_string();
   field_    = make_shared<EmpiricalFieldF>(std::move(field));
   calib_fmt = Vector2(M.calib_format().x, M.calib_format().y);
   set_working_format(M.working_format().x, M.working_format().y);
   init_approx_Ks_();
}

// ------------------------------------------------------------------------ init
//
void This::init(const Matrix3r& K)
{
   is_K_  = true;
   K_     = K;
   K_inv_ = K.inverse();
   field_ = make_shared<EmpiricalFieldF>();
   init_approx_Ks_();
}

// ------------------------------------------------------------------------ init
//
void This::init(const DistortionModel& M,
                const bool force_recalc,
                const AABBi aabb_in,
                const bool default_full_image,
                const float pixel_size_in,
                const bool feedback)
{
   if(!default_full_image) WARN(format("ignoring full-image=false setting"));

   const bool use_format  = true;
   const auto use_fmt_str = use_format ? "x"s : "o"s;

   const AABBi aabb       = aabb_in;
   const float pixel_size = pixel_size_in;

   const auto gen_aabb = aabb.area() == 0;
   const auto aabb_s
       = format("{}x{}x{}x{}", aabb.left, aabb.top, aabb.right, aabb.bottom);

   const auto A_str = M.to_json_string();
   // INFO(format("A-str = {}", A_str));

   const auto M_checksum1 = MD5(A_str).finalize().hexdigest();
   Expects(M_checksum1.size() == 32);
   const auto M_checksum
       = format("{}-{}", M_checksum1.substr(0, 16), M_checksum1.substr(16, 32));

   const auto key = format("inverse_{}_{}_{:4.2f}_{}",
                           M.sensor_id(),
                           (gen_aabb ? use_fmt_str : aabb_s),
                           pixel_size,
                           M_checksum);

   TRACE(format("key = {}, force = {}", key, str(force_recalc)));
   if(!force_recalc and detail::cu_store().load_cu(key, *this))
      return; // we're done

   bool loaded = false;

   const bool try_load
       = !force_recalc and data_source_exists(AssetType::CACHE_FILE, key);

   if(try_load) {
      // load it
      TRACE(format("try load: {}", key));

      try {
         fetch(*this, key);

         // Check
         DistortionModel M0;
         M0.from_json(str_to_json(A_str_));
         MatrixXr A    = M.A().array() - M0.A().array();
         const auto sq = A.squaredNorm();
         if(sq > 1e-6) {
            LOG_ERR("cache-file dirty: printing diagnostics");
            cout << "MA = " << endl << M.A() << endl << endl;
            cout << "A0 = " << endl << M0.A() << endl << endl;
            cout << "A  = " << endl << A << endl << endl;
            cout << "sq = " << str_precise(sq) << endl;
            throw std::runtime_error(format("cached file is dirty"));
         } else {
            TRACE(format("GOOD: sq = {}", str_precise(sq)));
         }

         loaded = true;
         if(feedback) {
            const auto err = distort_fidelity_check(*this, false);
            INFO(format("loaded '{}', with fidelity {}", key, err));
         } else if(multiview_trace_mode()) {
            const auto err = distort_fidelity_check(*this, true);
            TRACE(format("loaded '{}', with fidelity {}", key, err));
            // TRACE(format("aabb:  '{}'", aabb_.to_string()));
            // TRACE(format("scale: '{}'", str(scale_)));
            // cout << format("wxh    = {}x{}",
            //                field_->field.width,
            //                field_->field.height)
            //      << endl;
            // cout << format("ppt    = {}", str(field_->ppt)) << endl;
            // cout << format("fxy    = {}", str(field_->fxy)) << endl;
            // cout << format("domain = {}", str(field_->domain)) << endl;
            // auto print_coord = [&](int x, int y) {
            //    cout << format("[{}x{}] = U = {} -> D = {}",
            //                   x,
            //                   y,
            //                   str(field_->inv_transform(Vector2f(x, y))),
            //                   str(field_->field(x, y)))
            //         << endl;
            // };
            // print_coord(0, 0);
            // print_coord(field_->field.width - 1, field_->field.height - 1);
            // cout << endl;
         }
      } catch(std::exception& e) {
         WARN(format("failed to load cached-undistort: {}. (file-key = {})",
                     e.what(),
                     key));
      }
   }

   this->set_working_format(M.calib_format().x, M.calib_format().y);

   if(!loaded) {
      reinit(M, aabb, pixel_size, use_format, key); // calculate it
      store(*this, key);

      { // Sanity check...
         try {
            CachingUndistortInverse cu;
            fetch(cu, key);
            INFO(format("sanity check passed for cu file {}", key));
         } catch(std::exception& e) {
            FATAL(format("This is insane: just stored and reloaded and it "
                         "failed: {}",
                         e.what()));
         }
      }
   }

   this->set_working_format(M.working_format().x, M.working_format().y);
   detail::cu_store().store_cu(key, *this);

   if(false) { // Compare distort...
      const auto U  = M.undistort(Vector2(300, 200));
      const auto D1 = M.distort(U);
      const auto D2 = this->distort(U);
      cout << format("{}: U = {}, |{} - {}| = {}",
                     M.sensor_id(),
                     str(U),
                     str(D1),
                     str(D2),
                     (D1 - D2).norm())
           << endl;
   }
}

// ---------------------------------------------------------------------- reinit
//
void This::reinit(const DistortionModel& in_M,
                  const AABBi distorted_aabb,
                  const float pixel_size,
                  const bool use_format,
                  const string& key)
{
   const auto gen_aabb = (distorted_aabb.area() == 0);

   EmpiricalFieldF field;

   auto M = in_M;
   {
      M.finalize();
      const auto p = M.calib_format();
      M.set_working_format(unsigned(p.x), unsigned(p.y));
   }

   auto AABB_report = [](const string_view name, const AABB& aabb) {
      INFO(format("AABB report: {}", name));
      cout << format(R"V0G0N(
   [{}, {}, {}, {}]
   wxh = [{} x {}]
   area = {}
)V0G0N",
                     aabb.left,
                     aabb.top,
                     aabb.right,
                     aabb.bottom,
                     aabb.width(),
                     aabb.height(),
                     aabb.area());
   };

   AABB u_aabb = AABB::minmax();
   if(gen_aabb) {
      // The rectangle of the distorted image
      const auto d_format_i = M.calib_format();
      const auto d_format   = Vector2(d_format_i.x, d_format_i.y);
      Expects(d_format_i.x == 2592);
      Expects(d_format_i.y == 1944);

      const auto [K, K_err] = M.estimate_homography();

      if(false) {
         INFO(format("Distortiong K estimate is:"));
         cout << K << endl << endl;
         cout << format("K-err = {}", K_err) << endl << endl;
      }

      array<Vector2, 4> format_rect{{{0.0, 0.0},
                                     {d_format.x, 0.0},
                                     {d_format.x, d_format.y},
                                     {0.0, d_format.y}}};

      auto cr_aabb = M.calib_region();
      array<Vector2, 4> calib_rect{{{cr_aabb.left, cr_aabb.top},
                                    {cr_aabb.right, cr_aabb.top},
                                    {cr_aabb.right, cr_aabb.bottom},
                                    {cr_aabb.left, cr_aabb.bottom}}};

      auto f_aabb = AABB::minmax();
      for(const auto& d : format_rect) f_aabb.union_point(M.undistort(d));

      auto c_aabb = AABB::minmax();
      for(const auto& d : calib_rect) c_aabb.union_point(M.undistort(d));

      u_aabb = (use_format) ? f_aabb : c_aabb;

      // // Create the "large" u_aabb
      // // if(use_format)
      // //    for(const auto& d : format_rect)
      // u_aabb.union_point(M.undistort(d));
      // // else
      // //    for(const auto& d : calib_rect)
      // u_aabb.union_point(M.undistort(d));

      // // cout << format("large u-aabb = {}", str(u_aabb)) << endl;

      // LOG_ERR(format("M = {}", M.sensor_id()));
      // LOG_ERR(
      //     format("large c-aabb = {}, f-aabb = {}", str(c_aabb),
      //     str(f_aabb)));

      if(false) {
         const auto R = M.radial_epipole();
         cout << str(M) << endl;
         cout << format("WORKING FORMAT = {}", str(M.working_format())) << endl;
         AABB uc_aabb = AABB::minmax();
         for(const auto& x : M.calib_hull()) {
            const auto y = 0.90 * (x - R) + R;
            cout << format("x = {}   ==>   u = {}", str(y), str(M.undistort(y)))
                 << endl;
            uc_aabb.union_point(M.undistort(y));
         }

         AABB_report("f_aabb", f_aabb);
         AABB_report("c_aabb", c_aabb);
         AABB_report("uc_aabb", uc_aabb);
      }

      // LOG_ERR(format("uc-aabb = {}", str(uc_aabb)));

      if(false) {
         // Now shrink u_aabb to be the inscribed rectangle
         auto process = [&](const Vector2& p, int side) {
            if(side == 0 && p.y < u_aabb.top) u_aabb.top = p.y;
            if(side == 1 && p.x > u_aabb.right) u_aabb.right = p.x;
            if(side == 2 && p.y > u_aabb.bottom) u_aabb.bottom = p.y;
            if(side == 3 && p.x < u_aabb.left) u_aabb.left = p.x;
         };

         for(size_t side = 0; side < 4; ++side) {
            auto f = [&](int x, int y) {
               process(M.undistort(Vector2(x, y)), int(side));
            };
            if(use_format)
               bresenham(format_rect[side], format_rect[(side + 1) % 4], f);
            else
               bresenham(calib_rect[side], calib_rect[(side + 1) % 4], f);
         }
      }

      // cout << format("final u-aabb = {}", str(u_aabb)) << endl;

   } else {
      // Trace the edge of the distorted AABB, and make the AABB in the
      // undistorted region
      array<Point2, 4> quad{
          {Point2(distorted_aabb.left, distorted_aabb.bottom),
           Point2(distorted_aabb.left, distorted_aabb.top),
           Point2(distorted_aabb.right, distorted_aabb.top),
           Point2(distorted_aabb.right, distorted_aabb.bottom)}};

      auto process_xy = [&](int x, int y) {
         auto U = M.undistort(Vector2(x, y));
         u_aabb.union_point(U);
      };

      bresenham(quad[0], quad[1], process_xy);
      bresenham(quad[1], quad[2], process_xy);
      bresenham(quad[2], quad[3], process_xy);
      bresenham(quad[3], quad[0], process_xy);
   }

   // What is the granularity?
   const auto distort_w = (distorted_aabb.width() == 0)
                              ? real(M.calib_format().x)
                              : distorted_aabb.width();
   const unsigned w     = unsigned(std::ceil(distort_w / real(pixel_size)));
   const unsigned h     = unsigned(std::ceil(w / u_aabb.aspect_ratio()));
   field.ppt            = Vector2f(float(u_aabb.left), float(u_aabb.top));
   field.fxy            = to_vec2f(
       Vector2(u_aabb.width() / real(w - 1), u_aabb.height() / real(h - 1)));

   auto undist = [&](const Vector2f& U) -> Vector2f {
      auto D = M.distort(to_vec2(U));
      return to_vec2f(D);
   };

   // FATAL(format("u_aabb = {}", str(u_aabb)));

   static std::mutex padlock;
   {
      lock_guard<decltype(padlock)> lock(padlock);

      if(true) {
         INFO("Regenerating cached-distort function");
         cout << format("sensor = {}", M.sensor_id()) << endl;
         cout << format("aabb   = {} (gen = {})", str(u_aabb), str(gen_aabb))
              << endl;
         cout << format("u-wh   = {}, {}", u_aabb.width(), u_aabb.height())
              << endl;
         cout << format("aspect = {}", u_aabb.aspect_ratio()) << endl;
         cout << format("wxh    = {}x{}", w, h) << endl;
         cout << format("ppt    = {}", str(field.ppt)) << endl;
         cout << format("fxy    = {}", str(field.fxy)) << endl;
      }

      field.init(undist, u_aabb, w, true, true);
   }

   init(u_aabb, M, std::move(field));

   {
      const float err = float(distort_fidelity_check(*this, true));
      if(err > 2.0f * pixel_size) {
         // auto print_coord = [&](int x, int y) {
         //    INFO(format("[{}x{}] = U = {} -> D = {}",
         //                x,
         //                y,
         //                str(field_->inv_transform(Vector2f(x, y))),
         //                str(this->field_->field(x, y))));
         // };
         // print_coord(0, 0);
         // print_coord(w - 1, h - 1);

         FATAL(format(
             "Distortion fidelity is TERRIBLE, this should never happen."));
      }
   }
}

// ------------------------------------------------------ distort-fidelity-check
//
real This::distort_fidelity_check(const CachingUndistortInverse& in_cu,
                                  const bool feedback) noexcept
{
   // -- (*) -- We need to set the currect working format
   const Point2 native_format = in_cu.M().calib_format();
   CachingUndistortInverse cu_;
   const CachingUndistortInverse* cu_ptr = &in_cu;
   if((in_cu.working_format() - to_vec2(native_format)).norm() > 1e-5) {
      cu_ = in_cu;
      cu_.set_working_format(native_format.x, native_format.y);
      cu_ptr = &cu_;
   }
   Expects(cu_ptr != nullptr);
   const CachingUndistortInverse& cu = *cu_ptr;

   // -- (*) -- Get a set of 5 points
   array<Vector2f, 5> Ds;
   array<Vector2f, 5> Us;
   array<Vector2f, 5> Vs;
   array<real, 5> DV_err;

   const Vector2f C = 0.5f * to_vec2f(Point2(native_format.x, native_format.y));

   Ds[0] = C;
   Ds[1] = C + 0.25f * to_vec2f(Point2(native_format.x, native_format.y));
   Ds[2] = C + 0.25f * to_vec2f(Point2(native_format.x, -native_format.y));
   Ds[3] = C + 0.25f * to_vec2f(Point2(-native_format.x, native_format.y));
   Ds[4] = C + 0.25f * to_vec2f(Point2(-native_format.x, -native_format.y));

   std::transform(cbegin(Ds), cend(Ds), begin(Us), [&](const auto& D) {
      return to_vec2f(cu.undistort(to_vec2(D)));
   });

   std::transform(cbegin(Us), cend(Us), begin(Vs), [&](const auto& U) {
      return to_vec2f(cu.fast_distort(to_vec2(U)));
   });

   std::transform(cbegin(Ds),
                  cend(Ds),
                  cbegin(Vs),
                  begin(DV_err),
                  [](const Vector2f& a, const Vector2f& b) -> real {
                     return real((a - b).norm());
                  });

   const auto largest_itr = rng::smallest_elem(DV_err, std::greater<real>());
   Expects(largest_itr != cend(DV_err));

   if(feedback) {
      TRACE(format("Fidelity report for cu {}", cu.M().sensor_id()));
      const auto max_ind = std::distance(cbegin(DV_err), largest_itr);
      for(auto i = 0u; i < Ds.size(); ++i) {
         const char label = (i == max_ind) ? '*' : ' ';
         cout << format(" {:c} err = {} :: {} -> {} -> {} (slow-error "
                        "= {})",
                        label,
                        DV_err[i],
                        str(Ds[i]),
                        str(Us[i]),
                        str(Vs[i]),
                        (Ds[i] - cu.slow_distort(Us[i])).norm())
              << endl;
      }
   }

   return *largest_itr;
}

// ------------------------------------------------------------------- Load/Save
//
static const string k_caching_undistort_magic
    = "MagicCachingUndistortInverse_v6";

void load(CachingUndistortInverse& data, const string& fname) noexcept(false)
{
   using deleter_t = std::function<void(FILE*)>;
   unique_ptr<FILE, deleter_t> fp{fopen(fname.c_str(), "rb"),
                                  [](FILE* fp) { fclose(fp); }};

   if(!fp)
      throw std::runtime_error(
          format("failed to open '{}' for reading", fname));
   read(data, fp.get());
}

void save(const CachingUndistortInverse& data,
          const string& fname) noexcept(false)
{
   using deleter_t = std::function<void(FILE*)>;
   unique_ptr<FILE, deleter_t> fp{fopen(fname.c_str(), "wb"),
                                  [](FILE* fp) { fclose(fp); }};

   if(!fp)
      throw std::runtime_error(
          format("failed to open '{}' for writing", fname));
   write(data, fp.get());
}

void read(CachingUndistortInverse& data, FILE* fp) noexcept(false)
{
   string magic;
   load_str(fp, magic);

   if(magic != k_caching_undistort_magic)
      throw std::runtime_error(format("failed to read magic number"));

   AABB aabb;
   auto w = 0u;
   auto h = 0u;
   EmpiricalFieldF field;
   string A_str;
   string M_json_str;
   Point2 native_format{0, 0};
   Vector2 epipole{0.0, 0.0};
   real scale = 0.0;
   DistortionModel M;

   vector<Vector2f> Ds;
   vector<Vector2f> Us;
   vector<Vector2f> Vs;
   vector<float> dist;

   // Load the model
   load_str(fp, M_json_str);
   read(M, M_json_str);

   load_uint(fp, w);
   load_uint(fp, h);
   load_vec2f(fp, field.ppt);
   load_vec2f(fp, field.fxy);

   double val = 0.0;
   load_real(fp, aabb.left);
   load_real(fp, aabb.top);
   load_real(fp, aabb.right);
   load_real(fp, aabb.bottom);

   { // load the underlying model
      load_str(fp, A_str);
      load_int(fp, native_format.x);
      load_int(fp, native_format.y);
      load_real(fp, epipole.x);
      load_real(fp, epipole.y);
      load_real(fp, scale);
   }

   { // Let's load some results from the field.
      load_vec(fp, Ds, load_vec2f);
      load_vec(fp, Us, load_vec2f);
      load_vec(fp, Vs, load_vec2f);
      load_vec(fp, dist, load_float);
   }

   string hash = ""s;
   load_str(fp, hash);

   vector<uint32_t> buffer(2 * h * w);
   const auto n = fread(&buffer[0], sizeof(uint32_t), buffer.size(), fp);
   if(n != buffer.size())
      throw std::runtime_error(format(
          "read {} elements, but expected to read {}", n, buffer.size()));

   const auto calc_hash = md5(&buffer[0], buffer.size() * sizeof(uint32_t));
   if(hash != calc_hash) {
      throw std::runtime_error(
          format("hash sum failed: got {}, but expected {}", calc_hash, hash));
   }

   std::atomic<int> nfinite_counter{0};
   field.field.resize(w, h, w);
   auto& ff         = field.field;
   auto process_row = [w, &ff, &buffer, &nfinite_counter](int y) {
      size_t pos = size_t(y) * size_t(w) * 2;
      for(auto x = 0u; x < w; ++x) {
         Vector2f val;
         val.x = unpack_f32(boost::endian::little_to_native(buffer[pos++]));
         val.y = unpack_f32(boost::endian::little_to_native(buffer[pos++]));
         ff.pixels[x + size_t(y) * ff.row_stride] = val;
         if(!val.is_finite()) nfinite_counter++;
      }
   };

   ParallelJobSet pjobs;
   for(auto y = 0u; y < field.field.height; ++y)
      pjobs.schedule([&process_row, y]() { process_row(int(y)); });
   pjobs.execute();

   if(nfinite_counter > 0) {
      WARN(format("loaded {} non-finite field values", nfinite_counter));
   }

   if(false) {
      INFO("READ");
      cout << k_caching_undistort_magic << endl;
      // cout << data.A_str() << endl;
      cout << format("wxh  = [{}x{}]", w, h) << endl;
      cout << format("aabb = {}", str(aabb)) << endl;
      cout << format("ppt  = {}", str(field.ppt)) << endl;
      cout << format("fxy  = {}", str(field.fxy)) << endl;
      for(auto y = 0u; y < field.field.height; ++y) {
         for(auto x = 0u; x < field.field.width; ++x) {
            const auto& v = field.field(x, y);
            if(x == 10 and y == 10)
               printf("[%5.2f, %5.2f]\n", real(v.x), real(v.y));
         }
      }
      cout << endl;
   }

   data.init(aabb, M, std::move(field));

   {
      // CachingUndistortInverse cu_raw(data);
      CachingUndistortInverse& cu_raw = data;
      cu_raw.set_working_format(native_format.x, native_format.y);

      // WARN(format("cu-raw.id = {}", M.sensor_id()));

      auto fast_distort_2 = [&](const Vector2f& x) {
         auto f = cu_raw.field_->evaluate(x, true);
         auto X = to_vec2f(
             Vector2(real(f.x) * cu_raw.scale_.x, real(f.y) * cu_raw.scale_.y));
         cout << format(" >>> x = {}", str(x)) << endl;
         cout << format(" >>> f = {}", str(f)) << endl;
         cout << format(" >>> X = {}", str(X)) << endl;
         return X;
      };

      auto check_for_error = [&](const bool report) {
         bool has_error = false;
         {
            if(report) {
               INFO(format("LOAD REPORT, N = {}", Ds.size()));
               cout << format("native-format = {}", str(native_format)) << endl;
            }
            for(auto i = 0u; i < Ds.size(); ++i) {
               const Vector2f& D = Ds[i];
               const Vector2f U  = cu_raw.undistort(D);
               const Vector2f Ds = cu_raw.slow_distort(U);
               // const Vector2f Df = fast_distort_2(U);
               const Vector2f Df = cu_raw.fast_distort(U);
               const bool pass
                   = (D - Ds).norm() < 2.0f and (D - Df).norm() < 2.0f;
               if(!has_error) has_error = !pass;

               if(report) {
                  cout << format("  #{}... D = {}", i, str(D)) << endl;
                  cout << format("         U = |{} - {}| = {}\n",
                                 str(U),
                                 str(Us[i]),
                                 (U - Us[i]).norm());
                  cout << format("         Ds = |{} - {}| = {}\n",
                                 str(D),
                                 str(Ds),
                                 (D - Ds).norm());
                  cout << format("         Df = |{} - {}| = {}\n",
                                 str(D),
                                 str(Df),
                                 (D - Df).norm());
                  cout << format("      pass = {}\n", str(pass));

                  cout << endl;
               }
            }
         }
         return has_error;
      };

      const bool has_error = check_for_error(false);
      if(has_error) {
         WARN(format("Internal file-testcase failed in cache-undistort file, "
                     "sensor-id = '{}'",
                     M.sensor_id()));
         check_for_error(true);
         throw std::runtime_error("internal file-testcase failed");
      }
   }
}

void write(const CachingUndistortInverse& data, FILE* fp) noexcept(false)
{
   const auto& field = data.field();

   save_str(fp, k_caching_undistort_magic);

   // Save the model
   save_str(fp, data.M().to_json_string());

   save_uint(fp, field.field.width);
   save_uint(fp, field.field.height);
   save_vec2f(fp, field.ppt);
   save_vec2f(fp, field.fxy);

   save_real(fp, data.aabb().left);
   save_real(fp, data.aabb().top);
   save_real(fp, data.aabb().right);
   save_real(fp, data.aabb().bottom);

   const Point2 native_format = data.M().calib_format();
   const Vector2 epipole      = data.M().radial_epipole();
   const real scale           = data.M().scale();

   { // Save something about the underlying model
      save_str(fp, data.A_str());
      save_int(fp, native_format.x);
      save_int(fp, native_format.y);
      save_real(fp, epipole.x);
      save_real(fp, epipole.y);
      save_real(fp, scale);
   }

   { // Let's save some results from the field.
      // (The files stores it's own test-cases.)
      CachingUndistortInverse cu_raw(data);
      cu_raw.set_working_format(native_format.x, native_format.y);

      vector<Vector2f> Ds(5);
      vector<Vector2f> Us(5);
      vector<Vector2f> Vs(5);
      vector<float> dist(5);

      const Vector2f C
          = 0.5f * to_vec2f(Point2(native_format.x, native_format.y));

      Ds[0] = C;
      Ds[1] = C + 0.25f * to_vec2f(Point2(native_format.x, native_format.y));
      Ds[2] = C + 0.25f * to_vec2f(Point2(native_format.x, -native_format.y));
      Ds[3] = C + 0.25f * to_vec2f(Point2(-native_format.x, native_format.y));
      Ds[4] = C + 0.25f * to_vec2f(Point2(-native_format.x, -native_format.y));

      std::transform(cbegin(Ds), cend(Ds), begin(Us), [&](const auto& D) {
         return to_vec2f(cu_raw.undistort(to_vec2(D)));
      });

      std::transform(cbegin(Us), cend(Us), begin(Vs), [&](const auto& U) {
         return to_vec2f(cu_raw.fast_distort(to_vec2(U)));
      });

      for(auto i = 0u; i < Ds.size(); ++i) dist[i] = (Ds[i] - Vs[i]).norm();

      // Save the field values.
      save_vec(fp, cbegin(Ds), cend(Ds), save_vec2f);
      save_vec(fp, cbegin(Us), cend(Us), save_vec2f);
      save_vec(fp, cbegin(Vs), cend(Vs), save_vec2f);
      save_vec(fp, cbegin(dist), cend(dist), save_float);

      if(false) {
         INFO(format("SAVE REPORT, N = {}", Ds.size()));
         cout << format("native-format = {}", str(native_format)) << endl;
         for(auto i = 0u; i < Ds.size(); ++i) {
            cout << format("  #{}  |{} - {}| = {}, via {}",
                           i,
                           str(Ds[i]),
                           str(Vs[i]),
                           dist[i],
                           str(Us[i]))
                 << endl;
         }
         cout << format("   field(10, 10) = {}", str(field.field(10, 10)))
              << endl;
      }
   }

   vector<uint32_t> buffer(2 * field.field.height * field.field.width);
   size_t pos = 0;
   for(auto y = 0u; y < field.field.height; ++y) {
      for(auto x = 0u; x < field.field.width; ++x) {
         const Vector2f xy = field.field(x, y);
         buffer[pos++]     = boost::endian::native_to_little(pack_f32(xy.x));
         buffer[pos++]     = boost::endian::native_to_little(pack_f32(xy.y));
      }
   }

   const auto hash = md5(&buffer[0], buffer.size() * sizeof(uint32_t));
   save_str(fp, hash);

   const auto n = fwrite(&buffer[0], sizeof(uint32_t), buffer.size(), fp);
   if(n != buffer.size())
      throw std::runtime_error(format(
          "wrote {} elements, but expected to write {}", n, buffer.size()));

   if(false) {
      INFO("WRITE");
      cout << k_caching_undistort_magic << endl;
      //  cout << data.A_str() << endl;
      cout << format("wxh  = [{}x{}]", field.field.width, field.field.height)
           << endl;
      cout << format("aabb = {}", str(data.aabb())) << endl;
      cout << format("ppt  = {}", str(field.ppt)) << endl;
      cout << format("fxy  = {}", str(field.fxy)) << endl;
      for(auto y = 0u; y < field.field.height; ++y) {
         for(auto x = 0u; x < field.field.width; ++x) {
            const auto& v = field.field(x, y);
            printf("[%5.2f, %5.2f]  ", real(v.x), real(v.y));
         }
         cout << endl;
      }
   }
}

// -----------------------------------------------------------------------------
// --                       CachingUndistortInverse XDR                       --
// -----------------------------------------------------------------------------

void write_xdr(const CachingUndistortInverse& data, FILE* fp) noexcept(false)
{
   XdrWrapper xdrs(fp, XDR_ENCODE);
   const auto& field = data.field();
   unsigned w = field.field.width, h = field.field.height;

   xdr_encode(xdrs, k_caching_undistort_magic);
   xdr_encode(xdrs, data.aabb());
   xdr_encode(xdrs, data.A_str());
   xdr_encode(xdrs, w);
   xdr_encode(xdrs, h);
   xdr_encode(xdrs, field.ppt);
   xdr_encode(xdrs, field.fxy);
   for(auto y = 0u; y < h; ++y)
      for(auto x = 0u; x < w; ++x) xdr_encode(xdrs, field.field(x, y));

   for(auto y = 0u; y < field.field.height; ++y) {
      for(auto x = 0u; x < field.field.width; ++x) {
         const auto& v = field.field(x, y);
         printf("{%5.2f, %5.2f}  ", real(v.x), real(v.y));
      }
      cout << endl;
   }
}

void save_xdr(const CachingUndistortInverse& data,
              const string& fname) noexcept(false)
{
   using deleter_t = std::function<void(FILE*)>;
   unique_ptr<FILE, deleter_t> fp{fopen(fname.c_str(), "wb"),
                                  [](FILE* fp) { fclose(fp); }};
   if(!fp)
      throw std::runtime_error(
          format("failed to open '{}' for writing", fname));
   write_xdr(data, fp.get());
}

void read_xdr(CachingUndistortInverse& data, FILE* fp) noexcept(false)
{
   FATAL(format("funcitonality is now broken."));
   XdrWrapper xdrs(fp, XDR_DECODE);
   string magic;
   string A_str;
   unsigned w = 0u, h = 0u;
   AABB aabb;
   EmpiricalFieldF field;

   xdr_decode(xdrs, magic);
   if(magic != k_caching_undistort_magic)
      throw std::runtime_error(format("failed to read magic number"));

   xdr_decode(xdrs, aabb);
   xdr_decode(xdrs, A_str);
   xdr_decode(xdrs, w);
   xdr_decode(xdrs, h);
   xdr_decode(xdrs, field.ppt);
   xdr_decode(xdrs, field.fxy);

   field.field.resize(w, h, w);
   for(auto y = 0u; y < h; ++y)
      for(auto x = 0u; x < w; ++x) {
         Vector2f f;
         xdr_decode(xdrs, f);
         field.field(x, y) = f;
      }

   DistortionModel M;
   data.init(aabb, M, std::move(field));
}

void load_xdr(CachingUndistortInverse& data,
              const string& fname) noexcept(false)
{
   using deleter_t = std::function<void(FILE*)>;
   unique_ptr<FILE, deleter_t> fp{fopen(fname.c_str(), "rb"),
                                  [](FILE* fp) { fclose(fp); }};
   if(!fp)
      throw std::runtime_error(
          format("failed to open '{}' for reading", fname));
   read_xdr(data, fp.get());
}

// ---------------------------------------------------------------- Cached MapXY
//
static void get_cached_mapxy(const unsigned w,
                             const unsigned h,
                             const Matrix3r& H_in,
                             const CachingUndistortInverse& cu,
                             const Matrix3r& H_out,
                             cv::Mat& mapx,
                             cv::Mat& mapy,
                             const bool feedback)
{
   static std::unordered_map<string, array<cv::Mat, 2>> mapxy_dict;
   static std::mutex padlock;

   // Calculate the key
   auto str_it = [&](const Matrix3r& H) -> string {
      MD5 md5;
      for(auto r = 0; r < 3; ++r) {
         for(auto c = 0; c < 3; ++c) {
            auto v = H(r, c);
            md5.update(&v);
         }
      }
      md5.finalize();
      return md5.hexdigest();
   };
   const string key   = format("cached-mapxy_CU={}_{}x{}_Hin={}_Hout={}",
                             cu.A_str(),
                             w,
                             h,
                             str_it(H_in),
                             str_it(H_out));
   const string fname = format("{}/{}.cv", multiview_cache_dir(), key);

   { // (*) Have we already loaded mapx and mapy into memory?
      lock_guard<decltype(padlock)> lock(padlock);
      auto ii = mapxy_dict.find(key);
      if(ii != mapxy_dict.end()) {
         if(feedback) INFO(format("loading cached mapxy({}) from memory", key));
         mapx = ii->second[0];
         mapy = ii->second[1];
         return;
      }
   }

   // Function to save mapxy in the cache
   auto cache_mapxy = [&]() {
      lock_guard<decltype(padlock)> lock(padlock);
      mapxy_dict[key] = array<cv::Mat, 2>{{mapx, mapy}};
   };

   { // (*) Does mapxy exist on disk?
      if(is_regular_file(fname)) {
         if(feedback) INFO(format("loading cached mapxy({}) from disk", key));

         try {
            cv::FileStorage fs(fname.c_str(), cv::FileStorage::READ);
            fs["mapx"] >> mapx;
            fs["mapy"] >> mapy;
            cache_mapxy(); // Save to cache
            return;
         } catch(...) {
            FATAL(format("Input/ouput error"));
         }
      }
   }

   { // (*) We must calculate mapyxy, boo!
      if(feedback) INFO(format("recalculating cached mapxy({})", key));

      auto f = [&](const Vector2& x) { return cu.distort(x); };
      create_cv_remap(w, h, H_in, f, H_out, mapx, mapy);
      cache_mapxy(); // save to cache

      // Now write the files for next time...
      try {
         if(feedback) INFO(format("saving cached mapxy({}) to disk", key));
         cv::FileStorage fs(fname.c_str(), cv::FileStorage::WRITE);
         fs << "mapx" << mapx;
         fs << "mapy" << mapy;
      } catch(...) {
         FATAL(format("Input/output error"));
      }
   }
}

} // namespace perceive


#include "TriDualGraph.h"
#include <iostream>
#include <sstream>
#include <set>

#include <tbb/parallel_for.h>

#include <tbb/parallel_for_each.h>
#include <tbb/blocked_range2d.h>
#include <tbb/concurrent_vector.h>
//#include <Eigen/SparseCholesky>
//#include <Eigen/CholmodSupport>
//#include <Eigen/SparseLU>
#include <algorithm>
#include <Utils/LNormUtil.h>
#include <ctime>

#include <IO/MeshLoader.h>
#include <IO/MeshExporter.h>

using namespace Zephyr::Common;
//using namespace Zephyr::Graphics;

typedef Eigen::SparseMatrix<double> matrix;
typedef Eigen::Triplet<double> triplet;

Zephyr::Algorithm::TriDualGraph::TriDualGraph(Common::Mesh* pMesh) : mpMesh(pMesh)
{
	//ModelLoader loader;
	//auto filePath = "..\\model\\Lightning\\lightning_obj.obj";
	//auto filePath = "..\\model\\bunny.obj";
	//std::shared_ptr<RenderableModel> mpModel(new RenderableModel(L"bunny"));
	//mpModel->loadFromFile(filePath);

	//Common::Model model;
	//Common::MeshLoader::loadFile(filePath, &model);

	//auto mesh = model.getMesh(0);

	if (nullptr == mpMesh)
	{
		std::cout << "Failed to construct TriDualGraph : no mesh supplied" << std::endl;
		return;
	}

	build(*pMesh);

	//Common::Model resultModel;

	//MeshExporter::exportMesh("D:\\sandbox\\bunny_result.obj", &model);
}

Zephyr::Algorithm::TriDualGraph::~TriDualGraph()
{

}

void Zephyr::Algorithm::TriDualGraph::build(const Common::Mesh & mesh)
{
	const auto& indices = mesh.getIndices();
	const auto& vertices = mesh.getVertices();

	std::multimap<EdgeIdPair, int> edgesToNodeMap;

	// need to track Max   ||Ni - Nj||
	//                Edge            Inf
	float MaxE = 0;

	// create all the Node/Face
	for (int faceId = 0; faceId < mesh.getFaceCount(); ++faceId)
	{
		int currentId = faceId * 3;
		std::vector<int> index(3);
		index[0] = indices[currentId];
		index[1] = indices[currentId+1];
		index[2] = indices[currentId+2];

		auto point0 = vertices[index[0]];
		auto point1 = vertices[index[1]];
		auto point2 = vertices[index[2]];

		// add the node to the dual graph
		TriNode triNode(point0, point1, point2);
		int currentNodeId = addNode(triNode);

		// sort the indices in order, so edge id pair will match
		std::sort(index.begin(), index.end());

		// create the edges and link with other node
		for (int i = 0; i < 3; ++i)
		{
			EdgeIdPair edgePair;
			edgePair = ((i != 2) ? EdgeIdPair(index[i], index[i + 1]) : EdgeIdPair(index[0], index[2]));

			// find all the node that share this edge and link them
			auto relatedNodeIds = edgesToNodeMap.equal_range(edgePair);
			for (auto relatedNodeItr = relatedNodeIds.first; relatedNodeItr != relatedNodeIds.second; ++relatedNodeItr)
			{
				int relatedNodeId = relatedNodeItr->second;
				// link the two node
				int edgeId = linkNodes(relatedNodeId, currentNodeId);

				if (-1 == edgeId)
					continue;

				auto& edge = getEdge(edgeId);

				auto& node1 = getNode(currentNodeId);
				auto normal1 = node1.data.computeNormalNorm();
				auto& node2 = getNode(relatedNodeId);
				auto normal2 = node2.data.computeNormalNorm();

				//                 2
				// -B || Ni - Nj ||
				//                 Inf
				const float B = 1.0f;
				// Ni - Nj
				auto normalDiff = normal1 - normal2;
				//
				float infNorm = Common::LNormUtil::LInfinityNorm(normalDiff);
				float infNormSq = infNorm * infNorm;
				float upperTerm = (-1.0f * B * infNormSq);

				// need to track Max Edge LInfinityNorm
				MaxE = MaxE < infNorm ? infNorm : MaxE;

				// store the upper term first, later after all the edges is done, we get the lower term (depend on MaxE)
				edge.data.weight = upperTerm;
			}
			edgesToNodeMap.insert(std::make_pair(edgePair, currentNodeId));
		}
	}

	// for each edge, compute the actual weight
	// Actual weight = Exp ( UpperTerm / LowerTerm )
	// we already have UpperTerm computed above for each edge.data.weight
	float lowerTerm = MaxE;
	tbb::parallel_for_each(mEdges.begin(), mEdges.end(), [&](Edge& edge)
	{
		edge.data.weight = std::exp(edge.data.weight / lowerTerm);
	});
}

Zephyr::Common::Model Zephyr::Algorithm::TriDualGraph::segment(const std::vector<std::vector<int>>& inStrokes)
{
	/*
	// Data Term
	// Build X
	int numOfNode = (int)mNodes.size(); // n
	int numOfStrokes = (int)inStrokes.size(); // K

	matrix A(numOfNode, numOfNode);

	// X = n X K // label matrix
	matrix X(numOfNode, numOfStrokes);
	//tbb::concurrent_vector<triplet> XCoef;
	std::vector<triplet> XCoef;

	// Ip =  Ip(fi,fi)  =  1 if fi is marked by any label.
	matrix Ip(numOfNode, numOfNode);
	//tbb::concurrent_vector<triplet> IpCoef;
	std::vector<triplet> IpCoef;

	// Xc = label vector
	matrix Xc(numOfStrokes, numOfStrokes);
	//tbb::concurrent_vector<triplet> XcCoef;
	std::vector<triplet> XcCoef;

	// for each stroke, find the corresponding Xi and mark it
	//tbb::serial::parallel_for(0, (int)inStrokes.size(), [&](const int strokeId)
	for (int strokeId = 0; strokeId < (int)inStrokes.size(); ++strokeId)
	{
		auto& stroke = inStrokes[strokeId];
		//tbb::serial::parallel_for(0, (int)stroke.size(), [&](const int nodeId)
		for (int nodeId = 0; nodeId < (int)stroke.size(); ++nodeId)
		{
			XCoef.push_back(triplet(nodeId, strokeId, 1.0)); // mark the label vector
			IpCoef.push_back(triplet(nodeId, nodeId, 1.0)); // set the Ip matrix
		}//);
		XcCoef.push_back(triplet(strokeId, strokeId, 1.0));
	}//);
	X.setFromTriplets(XCoef.begin(), XCoef.end());
	XCoef.clear();
	Ip.setFromTriplets(IpCoef.begin(), IpCoef.end());
	IpCoef.clear();
	Xc.setFromTriplets(XcCoef.begin(), XcCoef.end());
	XcCoef.clear();
	// accumulate the result
	A = Ip;

	// clear Ip
	Ip.resize(0, 0);

	//std::cout << Xc << std::endl;

	// Propogation Term
	// build L
	// L = D - W

	// D
	matrix D(numOfNode, numOfNode);
	//tbb::concurrent_vector<triplet> DCoeff;
	std::vector<triplet> DCoeff;

	//tbb::serial::parallel_for(0, numOfNode, [&](const int i)
	for (int i = 0; i < numOfNode; ++i)
	{
		// summation of adjacency weight
		auto pEdges = getNeighbourEdges(i);

		float weight = 0;
		for (auto edge : pEdges)
		{
			weight += edge.data.weight;
		}

		// diagonal matrix
		DCoeff.push_back(triplet(i, i, weight));
	}//);
	D.setFromTriplets(DCoeff.begin(), DCoeff.end());
	DCoeff.clear();
	//std::cout << D;

	// W, weighted adjacency matrix
	matrix W(numOfNode, numOfNode);
	//tbb::concurrent_vector<triplet> WCoef;
	std::vector<triplet> WCoeff;
	//tbb::serial::parallel_for(0, (int)mEdges.size(), [&](const int edgeId)
	for (int edgeId = 0; edgeId < (int)mEdges.size(); ++edgeId)
	{
		auto edge = getEdge(edgeId);

		auto f1 = edge.nodeIds[0];
		auto f2 = edge.nodeIds[1];

		// W   = w    if (fi, fj) in E
		//  ij    ij
		WCoeff.push_back(triplet(f1, f2, edge.data.weight));
		// symmetry of the edge
		WCoeff.push_back(triplet(f2, f1, edge.data.weight));
	}//);
	W.setFromTriplets(WCoeff.begin(), WCoeff.end());
	WCoeff.clear();

	// L
	// L = D - W
	int c = 0;
	matrix L(numOfNode, numOfNode);
	//tbb::concurrent_vector<triplet> LCoef;
	std::vector<triplet> LCoeff;
	std::set<std::pair<int, int>> nonZeroIndex;

	// find non-zero in D
	for (int k = 0; k < D.outerSize(); ++k)
	{
		for (matrix::InnerIterator it(D, k); it; ++it)
		{
			nonZeroIndex.insert(std::make_pair(it.row(), it.col()));
		}
	}

	// find non-zero in W
	for (int k = 0; k < W.outerSize(); ++k)
	{
		for (matrix::InnerIterator it(W, k); it; ++it)
		{
			nonZeroIndex.insert(std::make_pair(it.row(), it.col()));
		}
	}

	for (auto i : nonZeroIndex)
	{
		LCoeff.push_back(triplet(i.first, i.second, D.coeff(i.first, i.second) - W.coeff(i.first, i.second)));
	}

	L.setFromTriplets(LCoeff.begin(), LCoeff.end());
	LCoeff.clear();

	// Clear D and W
	D.resize(0, 0);
	W.resize(0, 0);

	// acummulate into A
	A += L * L; // get L squared

	// Clear L
	L.resize(0, 0);

	// Gradient Term
	// Build S
	matrix S(numOfNode, numOfNode);
	//tbb::concurrent_vector<triplet> SCoef;
	std::vector<triplet> SCoeff;
	// set identity
	S.setIdentity();

	// mark each edge as -1.0f
	//tbb::serial::parallel_for(0, (int)mEdges.size(), [&](const int i)
	for (int i = 0; i < (int)mEdges.size(); ++i)
	{
		auto& edge = getEdge(i);

		auto f1 = edge.nodeIds[0];
		auto f2 = edge.nodeIds[1];

		SCoeff.push_back(triplet(f1, f2, -1.0));
		SCoeff.push_back(triplet(f2, f1, -1.0));
	}//);
	S.setFromTriplets(SCoeff.begin(), SCoeff.end());
	SCoeff.clear();

	A += 2 * (S * S);

	// Clear S
	S.resize(0, 0);

	// Build B
	// For each face that is labeled assign Xc(strokeId)
	matrix B(numOfNode, numOfStrokes);
	//tbb::concurrent_vector<triplet> BCoef;
	std::vector<triplet> BCoeff;
	//tbb::serial::parallel_for(0, (int)inStrokes.size(), [&](const int strokeId)
	for (int strokeId = 0; strokeId < (int)inStrokes.size(); ++strokeId)
	{
		auto& stroke = inStrokes[strokeId];
		//tbb::serial::parallel_for(0, (int)stroke.size(), [&](const int nodeId)
		for (auto nodeId : stroke)
		{
			for (int k = 0; k < numOfStrokes; ++k)
			{
				BCoeff.push_back(triplet(nodeId, k, Xc.coeff(strokeId, k)));
			}
		}//);
	}//);
	B.setFromTriplets(BCoeff.begin(), BCoeff.end());
	BCoeff.clear();

	// clear Xc
	Xc.resize(0, 0);

	// Formulate to solver
	// solve (Ip + L^2 + 2S^2)X = B
	std::cout << "Size of A : " << A.rows() << " X " << A.cols() << std::endl;
	std::cout << "Size of X : " << X.rows() << " X " << X.cols() << std::endl;
	std::cout << "Size of B : " << B.rows() << " X " << B.cols() << std::endl;

	//std::cout << B << std::endl;

	std::cout << "Building Cholesky... ";
	auto time = std::clock();

	Eigen::SparseMatrix<double> sparse = A;

	Eigen::CholmodDecomposition<Eigen::SparseMatrix<double>> chol;
	chol.analyzePattern(sparse);
	chol.factorize(sparse);

	std::cout << "Done in " << (std::clock() - time) / CLOCKS_PER_SEC << " Secs" << std::endl;

	std::cout << "Solving... ";
	time = std::clock();

	auto solveX = chol.solve(B);
	std::cout << "Done in " << (std::clock() - time) / CLOCKS_PER_SEC << " Secs" << std::endl;

	// evaluate the final result
	auto eval = solveX.eval();

	std::cout << eval.rows() << " X " << eval.cols() << std::endl;
	//std::cout << eval;

	std::cout << "Segmenting... ";
	time = std::clock();

	// Label the result
	std::vector<tbb::atomic<int>> segmentCounters(numOfStrokes);
	tbb::parallel_for(0, numOfNode, [&](const int i)
	//for (int i = 0; i < numOfNode; ++i)
	{
		double max = 0;
		int maxId = -1;

		for (int k = 0; k < numOfStrokes; ++k)
		{
			double value = std::abs(eval.coeff(i, k));

			maxId = value > max ? k : maxId;
			max = std::max(value, max);
		}

		mNodes[i].data.label = maxId;
		segmentCounters[maxId].fetch_and_increment();
	});
	std::cout << "Done in " << (std::clock() - time) / CLOCKS_PER_SEC << " Secs" << std::endl;

	// put the segmentation result into segments vector
	std::vector<tbb::concurrent_vector<int>> segments(numOfStrokes);
	for (int k = 0; k < numOfStrokes; ++k)
	{
		//std::cout << segmentCounters[k] << std::endl;
		segments[k].reserve(segmentCounters[k]);
	}

	tbb::parallel_for(0, numOfNode, [&](const int i)
	{
		auto label = mNodes[i].data.label;

		segments[label].push_back(i);
	});

	// Debug : print out the segment size
	for (int k = 0; k < numOfStrokes; ++k)
	{
		std::cout << segments[k].size() << std::endl;
	}

	// Construct the final result model
	// each label is a mesh

	Model resultModel;
	resultModel.resizeMeshes(numOfStrokes * 2); // give the user selected face a different color

	for (int label = 0; label < numOfStrokes; ++label)
	{
		const Vector3f labelColor = Vector3f((float)std::rand() / RAND_MAX, (float)std::rand() / RAND_MAX, (float)std::rand() / RAND_MAX);
		Material material;
		material.mDiffuseColor = labelColor * 0.5;

		std::stringstream ss;
		ss << label << "_mat";
		material.mName = ss.str();
		int currentMaterialId = resultModel.getMaterialsCount();
		resultModel.addMaterial(material);

		auto& mesh = resultModel.getMesh(label);
		mesh.setMaterialId(currentMaterialId);

		// setup the selected mesh
		Material selectedMaterial;
		std::stringstream ss2;
		ss2 << "selected_" << label << "_mat";
		selectedMaterial.mName = ss2.str();
		selectedMaterial.mDiffuseColor = labelColor;
		currentMaterialId = resultModel.getMaterialsCount();
		resultModel.addMaterial(selectedMaterial);
		auto& selectedMesh = resultModel.getMesh(numOfStrokes + label);
		selectedMesh.setMaterialId(currentMaterialId);

		auto userStrokes = inStrokes[label];
		std::set<int> strokeSet; // put in a set to speed up search
		strokeSet.insert(userStrokes.begin(), userStrokes.end());

		int userSelectedFaceCount = (int)userStrokes.size();
		selectedMesh.resizeIndices(userSelectedFaceCount * 3);
		auto& selectedIndices = selectedMesh.getIndices();

		auto& segment = segments[label];

		std::map<Vertex, int> vertexIdMap;
		std::map<Vertex, int> selectedVertexIdMap;

		// resize the index buffer
		mesh.resizeIndices(((int)segment.size() - userSelectedFaceCount) * 3);

		auto& indices = mesh.getIndices();

		int index = 0;
		int selectedIndex = 0;
		for (int i = 0; i < (int)segment.size(); ++i)
		{
			auto id = segment[i];
			auto& node = mNodes[id];

			// check if this face is user selected or not
			bool bIsSelected = false;
			if (strokeSet.find(id) != strokeSet.end())
				bIsSelected = true;

			int& localIndex = bIsSelected ? selectedIndex : index;
			auto& localIndices = bIsSelected ? selectedIndices : indices;
			auto& localVertexIdMap = bIsSelected ? selectedVertexIdMap : vertexIdMap;
			for (int k = 0; k < 3; ++k)
			{
				Vertex& v = node.data.mVertex[k];
				// set the label vertex color
				//v.color = labelColor;

				// check if this vertex already exist
				auto itr = localVertexIdMap.find(v);
				if (itr == localVertexIdMap.end())
					localVertexIdMap.insert(std::make_pair(v,(int)localVertexIdMap.size()));

				localIndices[localIndex * 3 + k] = localVertexIdMap[v];
			}
			++localIndex;
		}

		// insert the vertex buffer
		mesh.resizeVertices((int)vertexIdMap.size());
		auto& vertices = mesh.getVertices();
		tbb::parallel_for_each(vertexIdMap.begin(), vertexIdMap.end(), [&](const std::pair<Vertex, int>& itr)
		{
			vertices[itr.second] = itr.first;
		});

		selectedMesh.resizeVertices((int)selectedVertexIdMap.size());
		auto& selectedVertices = selectedMesh.getVertices();
		tbb::parallel_for_each(selectedVertexIdMap.begin(), selectedVertexIdMap.end(), [&](const std::pair<Vertex, int>& itr)
		{
			selectedVertices[itr.second] = itr.first;
		});
	}

	boost::filesystem::path outputPath("D:\\sandbox\\");
	std::stringstream ss;
	ss << "result" << "_bunny" << ".obj";
	outputPath /= ss.str();
	Common::MeshExporter::exportMesh(outputPath.string(), &resultModel);

	return resultModel;*/
	return Model();
}


#include <boost/filesystem.hpp>

#include "bvsma/include/functions.hpp"
#include "bvsma/include/ZKProofs.hpp"
#include "bvsma/include/SchnorrSignature.hpp"
#include "bvsma/include/fileFunctions.hpp"
#include "bvsma/include/functionsParallel.hpp"

using namespace boost::filesystem;

using namespace std;
typedef unsigned char byte;

/* Gen exp */

// ./server 0 192 501 1 BMDB 14 86
// ./server 0 224 501 1 BMDB 14 86
// ./server 0 256 501 1 BMDB 14 86

// ./server 0 192 501 1 PUT -53 136
// ./server 0 224 501 1 PUT -53 136
// ./server 0 256 501 1 PUT -53 136

// ./server 0 192 501 1 FRGC -1 75
// ./server 0 224 501 1 FRGC -1 75
// ./server 0 256 501 1 FRGC -1 75

/*
* SEMI-HONEST SERVER
*/

int main(int argc, char* argv[]){
    cout << "Run Semi-Honest Server ..." << endl;

    boost::asio::io_service io_service;

    auto clientIp = IpAddress::from_string("127.0.0.1");
	auto serverIp = IpAddress::from_string("127.0.0.1");

    SocketPartyData clientParty(clientIp, 1222);
    SocketPartyData serverParty(serverIp, 1223);

    shared_ptr<CommParty> server = make_shared<CommPartyTCPSynced>(io_service, serverParty, clientParty);

    boost::thread t(boost::bind(&boost::asio::io_service::run, &io_service));

    string dirServer = "../../../data/server/"+ string(argv[5]) +"/P"+ string(argv[2]);
    string enrLogTimer = dirServer +"/SHenrSer"+"P"+ string(argv[2])+ string(argv[5]) +".csv";
    string verLogTimer = dirServer +"/SHverSer"+"P"+ string(argv[2])+ string(argv[5]) +".csv";
    string keyStorage;
    string dirUser;

    string configFile = "../../../../libscapi/include/configFiles/NISTEC.txt";
    string curve = "P-"+string(argv[2]);

    auto dlog = make_shared<OpenSSLDlogECFp>(configFile, curve);
    auto g = dlog->getGenerator();
    auto q = dlog->getOrder();

    auto random = get_seeded_prg();

    shared_ptr<ElGamalOnGroupElementEnc> elgamalTH = make_shared<ElGamalOnGroupElementEnc>(dlog);

    unsigned int seedTH = 145523;

    int resultInt;
    bool resultBool;
    string str;

    std::chrono::time_point<std::chrono::system_clock> globalStart;
    std::chrono::time_point<std::chrono::system_clock> localStart;

    std::ofstream out_file;

    try
    {

        if (stoi(argv[1]) == 0)
        {

            int count = stoi(argv[3]);
            int step = stoi(argv[4]);

            server->join(500, 5000);

            for (size_t i = 1; i < count; i=i+step)
            {

                out_file.open(enrLogTimer, ios::app);
                if (!out_file)
                {
                    throw "Error creating file"s;
                }

                cout << "======= Enrollment =======" << endl;
                localStart = std::chrono::system_clock::now();
                globalStart = localStart;

                auto userId = receiveBigint(server);
                cout << "-> User " << userId << " Enrollment Request ..." << endl;
                dirUser = dirServer +"/SH/USERS/user"+userId.str();
                path pathUser(dirUser);
                create_directory(pathUser);
                dirUser = dirUser+"/";
                keyStorage = dirUser+"keystorage.txt";

                auto pairTH = elgamalTH->generateKey();
                auto s2 = dynamic_cast<ElGamalPrivateKey*>(pairTH.second.get())->getX();
                writeBigintinFile(keyStorage, "server", "PrivTH", s2);

                auto serverPKeyTH = pairTH.first;
                writePubKeyinFile(keyStorage, "server", "PubTH", serverPKeyTH);
                cout << "-> Server PrivTH and PubTH Generated and Stored ..." << endl;

                sendElGamalPubKey(server, serverPKeyTH);
                cout << "-> Server PubTH Sent ..." << endl;

                auto clientPKeyTH = receiveElGamalPubKey(server, dlog, elgamalTH);
                writePubKeyinFile(keyStorage, "client", "PubTH", clientPKeyTH);
                cout << "-> Client PubTH Received and Stored ..." << endl;

                auto serPKeyTH = dynamic_cast<ElGamalPublicKey*>(serverPKeyTH.get())->getH();
                auto cltPKeyTH = dynamic_cast<ElGamalPublicKey*>(clientPKeyTH.get())->getH();
                auto jointH = dlog->multiplyGroupElements(cltPKeyTH.get(),serPKeyTH.get());
                ElGamalPublicKey jointPK(jointH);
                shared_ptr<PublicKey> jointPubKey = make_shared<ElGamalPublicKey>(jointPK);
                elgamalTH->setKey(jointPubKey, pairTH.second);
                writePubKeyinFile(keyStorage, "joint", "jointTH", jointPubKey);
                writeCSVNew(out_file, localStart, false);
                cout << "-> Joint jointTH Received and stored ..." << endl;
                cout << "----> Keys Establishment DONE ..." << endl;

                cout << "-------------------------------------------" << endl;

                localStart = std::chrono::system_clock::now();
                auto userTemplate = receiveMapElGamalCipher(server, dlog, elgamalTH);

                writeTemplateSH(dirUser, userTemplate);

                writeCSVNew(out_file, localStart, false);
                writeCSVNew(out_file, globalStart, true);
                cout << "-> Template Received and Stored ..." << endl;
                cout << "======= Enrollment END =======" << endl;

            }

        }
        else if(stoi(argv[1]) == 1){

            server->join(500, 5000);

            int count = stoi(argv[3]);
            int step = stoi(argv[4]);

            for (size_t i = 1; i < count; i=i+step)
            {

                out_file.open(verLogTimer, ios::app);
                if (!out_file)
                {
                    throw "Error creating file"s;
                }
                cout << "======= Verification =======" << endl;
                localStart = std::chrono::system_clock::now();
                globalStart = localStart;

                cout << "--> Step 1: User's existence verification ..." << endl;
                auto userId = receiveBigint(server);
                dirUser = dirServer +"/SH/USERS/user"+userId.str()+"/";
                keyStorage = dirUser+"keystorage.txt";
                path p(dirUser+"Template.txt");
                if(!exists(p)){
                    server->writeWithSize("Unregistered");
                    throw "Unregistered User"s;
                }
                server->writeWithSize("Registered");
                cout << "User " << userId << " exists" << endl;
                writeCSVNew(out_file, localStart, false);

                localStart = std::chrono::system_clock::now();
                auto s2 = readBigintFromfile(keyStorage, "server", "PrivTH");
                ElGamalPrivateKey serPrivKeyTH(s2);
                shared_ptr<PrivateKey> serverPrivKeyTH = make_shared<ElGamalPrivateKey>(serPrivKeyTH);
                auto clientPKeyTH = readPubKeyFromfile(keyStorage, dlog, "client", "PubTH");
                auto serverPKeyTH = readPubKeyFromfile(keyStorage, dlog, "server", "PubTH");
                auto jointPubKey = readPubKeyFromfile(keyStorage, dlog, "joint", "jointTH");
                elgamalTH->setKey(jointPubKey, serverPrivKeyTH);

                /*  */

                biginteger threshold(stoi(argv[6]));
                size_t lenTH = stoi(argv[7]);
                auto thresholdEncSet = generateThresholdEncSet_parallel(seedTH, configFile, curve, jointPubKey, threshold, lenTH);
                // auto thresholdEncSet = generateThresholdEncSet(seedTH, dlog, elgamalTH, threshold, lenTH);

                cout << "-> Threshold Set Encrypted and Permuted ..." << endl;
                /*  */

                writeCSVNew(out_file, localStart, false);
                cout << "--> Keys and Threshold Set loaded ..." << endl;

                cout << "--> Step 1: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 2: Fetch Template ..." << endl;
                localStart = std::chrono::system_clock::now();
                //read map
                auto userTemplate = readTemplateSH(dirUser+"Template.txt", dlog) ;
                sendMapElGamalCipher(server, userTemplate);

                writeCSVNew(out_file, localStart, false);
                cout << "--> Step 2: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 3: Receive Final Score ..." << endl;
                localStart = std::chrono::system_clock::now();

                auto finalScore = receiveElGamalCipher(server, dlog, elgamalTH);

                writeCSVNew(out_file, localStart, false);
                cout << "--> Step 3: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 4: Comparison of the similarity score with the threshold set ..." << endl;
                localStart = std::chrono::system_clock::now();

                auto comparisonVect = subtractOneCipherFromCipherVect_parallel(configFile, curve, jointPubKey, thresholdEncSet, finalScore);
                auto blindRVect = generateVectBigintRand_parallel(q, comparisonVect.size());
                auto comparisonVectBlinded = blindVectCipherElGamal_parallel(configFile, curve, comparisonVect, blindRVect);
                auto partialDecBlindedCompVect = elgamalPartialDecryption_parallel(configFile, curve, jointPubKey, serverPrivKeyTH, comparisonVectBlinded);
                // auto comparisonVect = subtractOneCipherFromCipherVect(dlog, elgamalTH, thresholdEncSet, finalScore);
                // auto blindRVect = generateVectBigintRand(q, comparisonVect.size());
                // auto comparisonVectBlinded = blindVectCipherElGamal(dlog, comparisonVect, blindRVect);
                // auto partialDecBlindedCompVect = elgamalPartialDecryption(elgamalTH, comparisonVectBlinded);

                sendVectElGamalCipher(server, partialDecBlindedCompVect);

                writeCSVNew(out_file, localStart, false);
                cout << "--> Step 4: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 5: Comparison Result ..." << endl;
                localStart = std::chrono::system_clock::now();

                str = receiveStr(server);
                cout << str << endl;
                writeCSVNew(out_file, localStart, false);

                writeCSVNew(out_file, globalStart, true);
                cout << "--> Step 5: DONE ..." << endl;
                cout << endl;
                cout << "======= Verification END =======" << endl;
            }

        }

    }
    catch (const logic_error& e) {
        out_file.close();
        io_service.stop();
	    t.join();
		cerr << e.what();
	}
    catch (string &e){
        out_file.close();
        io_service.stop();
        t.join();
        cerr << e << endl;
    }

    io_service.stop();
    t.join();

    cout << "END Semi-Honest Server ..." << endl;
    return 0;

}


#pragma once

#include <Eigen/Core>

namespace Legolas{

  template <class T, int PACK_SIZE, int P2, int P3>
  double dot(const Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2> & right,
	     const Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2> & left){

    // double sum=0.0;
    // for (int i=0 ; i<PACK_SIZE ; i++){
    //   sum+=double(right(i))*double(left(i));
    // }

    //return sum;
    return right.matrix().dot(left.matrix());
  }

  //  template <class A>
  //  void  assignValueToScalar(double value, A & target){
  //    INFOS("ICI");
  //  }

  //  void assignValueToScalar(double value , Eigen::Array<float, 4, 1, 0, 4, 1> & target){
  //    INFOS("TOTO");
  //  }

  template <class T, int PACK_SIZE, int P2, int P3>
  void assignValueToScalar(double value,
  			   Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2>  & target){
    typedef Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2> ThisType;
    const ThisType packedValue(ThisType::Constant(value));
    //const Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2> packedValueConstant(value) ;
    //target=Eigen::Array<T, PACK_SIZE, 1>::Constant(value) ;
    target=packedValue;
    //target=ThisType::Constant(value);
  }

  template <class T, int PACK_SIZE, int P2, int P3>
  std::ostream& operator<< ( std::ostream& os,
			     const Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2> & v ){
    os << "|" ;
    for (int i=0 ; i<PACK_SIZE ; i++){
      os << v(i) ;
      if (i!=PACK_SIZE-1) os <<" ";
    }
    os << "|" ;
    return os ;
  }

  template <class T, int PACK_SIZE, int P2, int P3>
  void inverse(Eigen::Array<T, PACK_SIZE, P2, P3, PACK_SIZE,P2> & a){a=a.inverse();}

}


// Boost.Geometry Index
//
// R-tree strategies
//
// Copyright (c) 2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INDEX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INDEX_HPP

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/spherical/distance_segment_box.hpp>
#include <boost/geometry/strategies/spherical/envelope_box.hpp>
#include <boost/geometry/strategies/spherical/envelope_point.hpp>
#include <boost/geometry/strategies/spherical/envelope_segment.hpp>
#include <boost/geometry/strategies/spherical/expand_box.hpp>
#include <boost/geometry/strategies/spherical/expand_point.hpp>
#include <boost/geometry/strategies/spherical/expand_segment.hpp>
#include <boost/geometry/strategies/spherical/intersection.hpp>
#include <boost/geometry/strategies/spherical/point_in_poly_winding.hpp>

#include <boost/geometry/strategies/index.hpp>

namespace boost { namespace geometry { namespace strategy { namespace index
{

template
<
    typename CalculationType = void
>
struct spherical
{
    typedef spherical_tag cs_tag;

    typedef geometry::strategy::envelope::spherical_point envelope_point_strategy_type;
    typedef geometry::strategy::envelope::spherical_box envelope_box_strategy_type;
    typedef geometry::strategy::envelope::spherical_segment
        <
            CalculationType
        > envelope_segment_strategy_type;

    static inline envelope_segment_strategy_type get_envelope_segment_strategy()
    {
        return envelope_segment_strategy_type();
    }

    typedef geometry::strategy::expand::spherical_point expand_point_strategy_type;
    typedef geometry::strategy::expand::spherical_box expand_box_strategy_type;
    typedef geometry::strategy::expand::spherical_segment
        <
            CalculationType
        > expand_segment_strategy_type;

    static inline expand_segment_strategy_type get_expand_segment_strategy()
    {
        return expand_segment_strategy_type();
    }

	typedef geometry::strategy::covered_by::spherical_point_box covered_by_point_box_strategy_type;
	typedef geometry::strategy::covered_by::spherical_box_box covered_by_box_box_strategy_type;
	typedef geometry::strategy::within::spherical_point_point within_point_point_strategy_type;
	//typedef geometry::strategy::within::spherical_point_box within_point_box_strategy_type;
	//typedef geometry::strategy::within::spherical_box_box within_box_box_strategy_type;

    // used in equals(Seg, Seg) but only to get_point_in_point_strategy()
    typedef geometry::strategy::intersection::spherical_segments
        <
            CalculationType
        > relate_segment_segment_strategy_type;

    static inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy()
    {
        return relate_segment_segment_strategy_type();
    }

    // used in intersection_content
    typedef geometry::strategy::disjoint::spherical_box_box disjoint_box_box_strategy_type;

    typedef geometry::strategy::distance::comparable::haversine
        <
            double,
            CalculationType
        > comparable_distance_point_point_strategy_type;

    static inline comparable_distance_point_point_strategy_type get_comparable_distance_point_point_strategy()
    {
        return comparable_distance_point_point_strategy_type();
    }

    // TODO: Comparable version should be possible
    typedef geometry::strategy::distance::cross_track_point_box
        <
            CalculationType,
            geometry::strategy::distance::haversine<double, CalculationType>
        > comparable_distance_point_box_strategy_type;

    static inline comparable_distance_point_box_strategy_type get_comparable_distance_point_box_strategy()
    {
        return comparable_distance_point_box_strategy_type();
    }

    // TODO: Radius is not needed in comparable strategy
    typedef geometry::strategy::distance::comparable::cross_track
        <
            CalculationType,
            geometry::strategy::distance::comparable::haversine<double, CalculationType>
        > comparable_distance_point_segment_strategy_type;

    static inline comparable_distance_point_segment_strategy_type get_comparable_distance_point_segment_strategy()
    {
        return comparable_distance_point_segment_strategy_type();
    }

    // comparable?
    typedef geometry::strategy::distance::spherical_segment_box
        <
            CalculationType,
            geometry::strategy::distance::haversine<double, CalculationType>
        > comparable_distance_segment_box_strategy_type;

    static inline comparable_distance_segment_box_strategy_type get_comparable_distance_segment_box_strategy()
    {
        return comparable_distance_segment_box_strategy_type();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, spherical_tag>
{
    typedef spherical<> type;
};

template <typename Geometry>
struct default_strategy<Geometry, spherical_polar_tag>
{
    typedef spherical<> type;
};

template <typename Geometry>
struct default_strategy<Geometry, spherical_equatorial_tag>
{
    typedef spherical<> type;
};

template <typename Point1, typename Point2, typename CalculationType>
struct from_strategy<within::spherical_winding<Point1, Point2, CalculationType> >
{
    typedef strategy::index::spherical<CalculationType> type;

    static inline type get(within::spherical_winding<Point1, Point2, CalculationType> const&)
    {
        return type();
    }
};

// distance (MPt, MPt)
template <typename RadiusType, typename CalculationType>
struct from_strategy<distance::comparable::haversine<RadiusType, CalculationType> >
{
    typedef strategy::index::spherical<CalculationType> type;

    static inline type get(distance::comparable::haversine<RadiusType, CalculationType> const&)
    {
        return type();
    }
};

// distance (MPt, Linear/Areal)
template <typename CalculationType, typename PPStrategy>
struct from_strategy<distance::comparable::cross_track<CalculationType, PPStrategy> >
{
    typedef strategy::index::spherical<CalculationType> type;

    static inline type get(distance::comparable::cross_track<CalculationType, PPStrategy> const&)
    {
        return type();
    }
};

} // namespace services

}}}} // namespace boost::geometry::strategy::index

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INDEX_HPP


#include "../agg.hpp"
#include <boost/accumulators/statistics/variance.hpp>

ARRAY_AGGREGATE_FNC(avariance, tag::variance);
SQL_AGGREGATE_FNC(v_variance, tag::variance);


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "reverb_object_renderer.hpp"

#include <libvisr/signal_flow_context.hpp>

#include <libefl/db_linear_conversion.hpp>
#include <libefl/basic_matrix.hpp>
#include <libefl/vector_functions.hpp>

#include <libpanning/LoudspeakerArray.h>

#include <libpml/empty_parameter_config.hpp>

#include <librbbl/fft_wrapper_factory.hpp>
#include <librbbl/fft_wrapper_base.hpp>

#include <librcl/fir_filter_matrix.hpp>
#include <librcl/crossfading_fir_filter_matrix.hpp>

#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include <algorithm>
#include <cmath>
#include <vector>

// If active, the matrix of generated late reverb decorrelation filters is
// written into a text file in working directory of the application.
#define DEBUG_DECORR_FILTER_GENERATION

#ifdef DEBUG_DECORR_FILTER_GENERATION
#include <iterator>
#include <fstream>
#endif

#include <algorithm>
#include <cmath>

namespace visr
{
namespace reverbobject
{

namespace // unnamed
{
  /**
   * Create a routing that routes \p numChannels input signals to a single output (adding them).
   * @param numChannels The number of input signals, which are translated to the indices 0..numChanels-1
   * @todo Consider to make this an utility function (e.g., in librbbl)
   */
  rbbl::FilterRoutingList allToOneRouting( std::size_t numChannels )
  {
    rbbl::FilterRoutingList res;
    for (std::size_t chIdx(0); chIdx < numChannels; ++chIdx)
    {
      res.addRouting(chIdx, 0, chIdx, 1.0f);
    }
    return res;
  }

  /**
   * Create a routing that routes a single input to \p numchannels output signals, filtering each with an individual filter.
   * @param numChannels The number of output signals, which are translated to the indices 0..numChannels-1
   * @todo Consider to make this an utility function (e.g., in librbbl)
   */
  rbbl::FilterRoutingList oneToAllRouting(std::size_t numChannels)
  {
    rbbl::FilterRoutingList res;
    for (std::size_t chIdx(0); chIdx < numChannels; ++chIdx)
    {
      res.addRouting(0, chIdx, chIdx, 1.0f);
    }
    return res;
  }

  /**
   * Create a set of random-phase allpass filters. The frequency response is unity at the frequency grid points, but the phase is unformly
   * distributed in [-pi,pi].
   * @param filters The matrix to be filled. Must be set to the desired dimensions beforehand.
   * @todo Consider promoting this into a general-purpose library function (rbbl or efl).
   */
  template<typename DataType>
  void generateRandomPhaseAllpass(efl::BasicMatrix<DataType> & filters)
  {
    std::size_t const numFilters{ filters.numberOfRows() };
    std::size_t const filterLength{ filters.numberOfColumns() };
    std::size_t const freqResponseSize{ filterLength / 2 + 1 }; // Frequency

    std::vector<std::complex<DataType> > freqResponse(freqResponseSize);
    // 0-Hz and fs/2 frequency bins must be real-valued for a real-valued
    freqResponse.front() = std::complex<DataType>{ 1.0f, 0.0 };
    freqResponse.back() = std::complex<DataType>{ 1.0f, 0.0 };

    std::unique_ptr<rbbl::FftWrapperBase<DataType> >  fft
      = rbbl::FftWrapperFactory<DataType>::create("default", filterLength, 0 /*no alignment specified*/);
    DataType const scaleFactor{ static_cast<DataType>(1.0) / (static_cast<DataType>(filterLength) * fft->inverseScalingFactor()) };

    boost::random::mt19937 gen;
    boost::random::uniform_real_distribution<DataType> uniformPhaseGen{ -boost::math::constants::pi<DataType>(), boost::math::constants::pi<DataType>() };

#ifdef DEBUG_DECORR_FILTER_GENERATION
    std::stringstream fileName;
    fileName << "leta_decorrelation_filters.dat";
    std::ofstream out(fileName.str(), std::ios_base::out);
#endif

    for (std::size_t filterIdx{ 0 }; filterIdx < numFilters; ++filterIdx)
    {
      std::generate( freqResponse.begin()+1, freqResponse.end()-1,
        [&]()
        {
          DataType const ang{ uniformPhaseGen(gen) };
          std::complex<DataType> const val{ std::cos(ang), std::sin(ang) };
          return val;
        }
      );
      fft->inverseTransform( &freqResponse[0], filters.row(filterIdx));
      efl::ErrorCode const res = efl::vectorMultiplyConstantInplace<DataType>( scaleFactor, filters.row(filterIdx), filterLength, 0/*no alignment*/);
      if (res != efl::noError)
      {
        throw std::runtime_error(detail::composeMessageString("Error creating an random-phase allpass:", efl::errorMessage(res) ) );
      }

#ifdef DEBUG_DECORR_FILTER_GENERATION
      std::copy(filters.row(filterIdx), filters.row(filterIdx) + filterLength, std::ostream_iterator<DataType>(out, " "));
      out << "\n";
#endif
    }
  }

} // unnamed namespace

ReverbObjectRenderer::ReverbObjectRenderer( SignalFlowContext const & context,
                                      char const * name,
                                      CompositeComponent * parent,
                                      std::string const & reverbConfig,
                                      panning::LoudspeakerArray const & arrayConfig,
                                      std::size_t numberOfObjectSignals )
  : CompositeComponent( context, name, parent )
  , mObjectSignalInput( "in", *this, numberOfObjectSignals )
  , mLoudspeakerOutput( "out", *this, arrayConfig.getNumRegularSpeakers() )
  , mObjectVector( "objectIn", *this, pml::EmptyParameterConfig() )
  , mReverbParameterCalculator( context, "parameterCalculator", this )
  , mReverbSignalRouting( context, "signalRouting", this )
  , mDiscreteReverbDelay( context, "discreteReverbDelay", this )
  , mDiscreteReverbReflFilters( context, "discreteReverbReflectionFilters", this )
  , mDiscreteReverbPanningMatrix( context, "discretePanningMatrix", this )
  , mLateReverbFilterCalculator()
  , mLateReverbGainDelay( context, "lateReverbGainDelay", this )
  , mReverbMix( context, "Sum", this, arrayConfig.getNumRegularSpeakers(), 2 )
{
  // Parse and apply default values for the reverb parameters.
  std::stringstream stream( reverbConfig );
  boost::property_tree::ptree tree;
  try
  {
    read_json( stream, tree );
  }
  catch( std::exception const & ex )
  {
    throw std::invalid_argument( std::string( "Error while parsing reverb config string: " ) + ex.what() );
  }

  std::size_t const maxNumReverbObjects = tree.get<std::size_t>( "numReverbObjects", 0 );
  SampleType const lateReverbFilterLengthSeconds = tree.get<SampleType>( "lateReverbFilterLength", 0 );
  // Enforce a minimum late reverb filter length of 1 sample. This is currently needed as not all components are safe when used with a filter length of 0.
  std::size_t const lateReverbFilterLengthSamples = std::max( static_cast<std::size_t>(std::ceil( lateReverbFilterLengthSeconds * samplingFrequency() )),
    static_cast<std::size_t>(1) );

  std::string const lateReverbDecorrFilterName( tree.get<std::string>( "lateReverbDecorrelationFilters", std::string() ));
  std::size_t const numDiscreteReflectionsPerObject = tree.get<std::size_t>( "discreteReflectionsPerObject", 0 );

  std::size_t const numWallReflBiquads = objectmodel::PointSourceWithReverb::cNumDiscreteReflectionBiquads;

  SampleType const maxDiscreteReflectionDelay = tree.get<SampleType>( "maxDiscreteReflectionDelay",  1.0f );

  SampleType const lateReverbCrossfadeTime = tree.get<SampleType>("lateReverbCrossfadeTime", 2.0f); // Default value
  std::size_t const lateReverbCrossfadeSamples{ static_cast<std::size_t>(std::max(0.0f, std::ceil(lateReverbCrossfadeTime*context.samplingFrequency()) )) };

  // The maximum number of late filter recalculations per period.
  std::size_t const cLateFilterUpdatesPerPeriod( tree.get<std::size_t>( "lateReverbFilterUpdatesPerPeriod", 1 ));

  // Optional argument for the gain of the decorrelation filter for
  // the late reverb tail.
  // The default value is 1/sqrt(#loudspeakers)
  boost::optional<SampleType> const lateReverbDecorrelatorGainOpt = tree.get_optional<SampleType>( "lateReverbDecorrelationGain" );
  SampleType const defaultLateDecorrelatorGain = 1.0f / std::sqrt( static_cast<float>(arrayConfig.getNumRegularSpeakers()) );
  SampleType const lateReverbDecorrelatorGain = lateReverbDecorrelatorGainOpt
    ? efl::dB2linear( *lateReverbDecorrelatorGainOpt ) : defaultLateDecorrelatorGain;

  pml::MatrixParameter<SampleType> lateDecorrelationFilters(cVectorAlignmentSamples );
    if( lateReverbDecorrFilterName.empty() )
    {
      lateDecorrelationFilters.resize( arrayConfig.getNumRegularSpeakers(), 512 ); // default filter length
      generateRandomPhaseAllpass( lateDecorrelationFilters );
    }
    else
    {
      boost::filesystem::path const lateReverbDecorrFilterPath( lateReverbDecorrFilterName );
      if( not exists( lateReverbDecorrFilterPath ) )
      {
        throw std::invalid_argument( "The file path \"lateReverbDecorrelationFilters\" provided in the reverb configuration does not exist." );
      }
      pml::MatrixParameter<SampleType> const allLateDecorrelationFilters = pml::MatrixParameter<SampleType>::fromAudioFile( lateReverbDecorrFilterName, cVectorAlignmentSamples );
      std::size_t const lateDecorrelationFilterLength = allLateDecorrelationFilters.numberOfColumns();
      if( allLateDecorrelationFilters.numberOfRows() < arrayConfig.getNumRegularSpeakers() )
      {
        throw std::invalid_argument( "The number of loudspeakers exceeds the number of late reverberation decorrelation filters in the provided file." );
      }
      lateDecorrelationFilters.resize( arrayConfig.getNumRegularSpeakers(), lateDecorrelationFilterLength );
      for( std::size_t rowIdx( 0 ); rowIdx < arrayConfig.getNumRegularSpeakers(); ++rowIdx )
      {
        // Multiply the raw unit-magnitude filters by the scaling gain.
        if( efl::vectorMultiplyConstant( lateReverbDecorrelatorGain,
                                        allLateDecorrelationFilters.row( rowIdx ),
                                        lateDecorrelationFilters.row( rowIdx ),
                                        lateDecorrelationFilterLength,
                                        cVectorAlignmentSamples ) != efl::noError )
        {
          throw std::runtime_error( "Copying and scaling of late decorrelation filter rows failed." );
        }
      }

    }

    std::size_t const interpolationSteps = period();

    // set up the components
    mReverbSignalRouting.setup( numberOfObjectSignals, maxNumReverbObjects, true /*contronInputs*/ );

    mReverbParameterCalculator.setup( arrayConfig, maxNumReverbObjects,
                                      numDiscreteReflectionsPerObject,
                                      numWallReflBiquads,
                                      lateReverbFilterLengthSeconds,
                                      objectmodel::PointSourceWithReverb::cNumberOfSubBands );
    mDiscreteReverbDelay.setup( maxNumReverbObjects*numDiscreteReflectionsPerObject,
                                interpolationSteps, maxDiscreteReflectionDelay, "lagrangeOrder3",
                                rcl::DelayVector::MethodDelayPolicy::Limit,
                                rcl::DelayVector::ControlPortConfig::All, 0.0f, 0.0f );
    mDiscreteReverbReflFilters.setup( maxNumReverbObjects*numDiscreteReflectionsPerObject, numWallReflBiquads, true /*controlInputs*/ );
    mDiscreteReverbPanningMatrix.setup( maxNumReverbObjects*numDiscreteReflectionsPerObject,
                                        arrayConfig.getNumRegularSpeakers(),
                                        interpolationSteps );
    mLateReverbFilterCalculator.reset( new LateReverbFilterCalculator( context, "lateReverbCalculator", this,
                                       maxNumReverbObjects, lateReverbFilterLengthSeconds,
                                       objectmodel::PointSourceWithReverb::cNumberOfSubBands,
                                       cLateFilterUpdatesPerPeriod ) );

  // TODO: Add configuration parameters for maximum delay of the late reverb onset delay.
  mLateReverbGainDelay.setup( maxNumReverbObjects,
                              period(),
                              maxDiscreteReflectionDelay, // For the moment, use the same max. delay as for discretes.
                              "lagrangeOrder3",
                              rcl::DelayVector::MethodDelayPolicy::Limit,
                              rcl::DelayVector::ControlPortConfig::All,
                              0.0f, 0.0f );

  // A single late reverb filter for each object, which are routed (summed) to a single output.
  if( lateReverbCrossfadeSamples > 0 )
  {
    mLateReverbFilter.reset(new rcl::CrossfadingFirFilterMatrix(context,
                             "lateReverbFilter",
                              this,
                              maxNumReverbObjects,
                              1,
                              lateReverbFilterLengthSamples,
                              maxNumReverbObjects,
                              maxNumReverbObjects,
                              lateReverbCrossfadeSamples,
                              efl::BasicMatrix<SampleType>(), // No initial filters provided.
                              allToOneRouting(maxNumReverbObjects),
                              rcl::CrossfadingFirFilterMatrix::ControlPortConfig::Filters, "default"
                             ));
  }
  else
  {
    mLateReverbFilter.reset(new rcl::FirFilterMatrix(context,
                             "lateReverbFilter",
                             this,
                             maxNumReverbObjects,
                             1,
                             lateReverbFilterLengthSamples,
                             maxNumReverbObjects,
                             maxNumReverbObjects,
                            efl::BasicMatrix<SampleType>(), // No initial filters provided.
                            allToOneRouting(maxNumReverbObjects),
                            rcl::FirFilterMatrix::ControlPortConfig::Filters, "default"
    ));
  }

  // Create #loudspeakers decorrelated signals from a single, each filtered with an individual decorrelation filter
  mLateDiffusionFilter.reset(new rcl::FirFilterMatrix(context, "decorrelationFilter", this,
                             1, arrayConfig.getNumRegularSpeakers( ), lateDecorrelationFilters.numberOfColumns(),
                             arrayConfig.getNumRegularSpeakers( ), arrayConfig.getNumRegularSpeakers( ),
                             lateDecorrelationFilters, oneToAllRouting( arrayConfig.getNumRegularSpeakers()) ) );

  audioConnection( mObjectSignalInput, mReverbSignalRouting.audioPort("in") );
  std::size_t const totalDiscreteReflections = maxNumReverbObjects*numDiscreteReflectionsPerObject;
  std::vector<ChannelList::IndexType> discreteFanOut( totalDiscreteReflections );
  // Construct a sequence of indices [0 0 .. 0 1 1 ..1 M-1 M-1 .. M-1] with M = maxNumReverbObjects and
  // numDiscreteReflectionsPerObject repetitions each.
  for( std::size_t chIdx(0); chIdx < totalDiscreteReflections; ++chIdx )
  {
    discreteFanOut[chIdx] = chIdx / numDiscreteReflectionsPerObject;
  }
  audioConnection( mReverbSignalRouting.audioPort("out"), ChannelList(discreteFanOut),
                   mDiscreteReverbDelay.audioPort("in"), ChannelRange(0,totalDiscreteReflections) );
  audioConnection( mDiscreteReverbDelay.audioPort("out"), mDiscreteReverbReflFilters.audioPort("in") );
  audioConnection( mDiscreteReverbReflFilters.audioPort("out"), mDiscreteReverbPanningMatrix.audioPort("in") );
  audioConnection( mDiscreteReverbPanningMatrix.audioPort("out"), mReverbMix.audioPort("in0") );

  audioConnection( mReverbSignalRouting.audioPort("out"), mLateReverbGainDelay.audioPort("in") );
  audioConnection( mLateReverbGainDelay.audioPort("out"), mLateReverbFilter->audioPort("in") );
  audioConnection( mLateReverbFilter->audioPort("out"), mLateDiffusionFilter->audioPort("in") );
  audioConnection( mLateDiffusionFilter->audioPort("out"), mReverbMix.audioPort("in1") );

  audioConnection( mReverbMix.audioPort("out"), mLoudspeakerOutput );

  parameterConnection( mObjectVector, mReverbParameterCalculator.parameterPort("objectInput") );
  parameterConnection( mReverbParameterCalculator.parameterPort("signalRoutingOut"), mReverbSignalRouting.parameterPort("controlInput") );
  parameterConnection( mReverbParameterCalculator.parameterPort("discreteGainOut"), mDiscreteReverbDelay.parameterPort( "gainInput" ) );
  parameterConnection( mReverbParameterCalculator.parameterPort("discreteDelayOut"), mDiscreteReverbDelay.parameterPort( "delayInput" ) );
  parameterConnection( mReverbParameterCalculator.parameterPort("discreteEqOut"), mDiscreteReverbReflFilters.parameterPort( "eqInput" ) );
  parameterConnection( mReverbParameterCalculator.parameterPort("discretePanningGainOut"), mDiscreteReverbPanningMatrix.parameterPort( "gainInput" ) );
  parameterConnection( mReverbParameterCalculator.parameterPort("lateGainOut"), mLateReverbGainDelay.parameterPort( "gainInput" ) );
  parameterConnection( mReverbParameterCalculator.parameterPort("lateDelayOut"), mLateReverbGainDelay.parameterPort( "delayInput" ) );

  parameterConnection( mReverbParameterCalculator.parameterPort("lateSubbandOut"), mLateReverbFilterCalculator->parameterPort("subbandInput") );
  parameterConnection( mLateReverbFilterCalculator->parameterPort("lateFilterOutput"), mLateReverbFilter->parameterPort("filterInput") );

}

ReverbObjectRenderer::~ReverbObjectRenderer() = default;

} // namespace reverbobject
} // namespace visr


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/boost/fusion/list.hpp>

#include <boost/hana/tuple.hpp>

#include <laws/applicative.hpp>
#include <laws/base.hpp>
#include <laws/functor.hpp>
#include <laws/monad.hpp>

#include <boost/fusion/container/generation/make_list.hpp>
#include <boost/fusion/container/list.hpp>
using namespace boost::hana;
namespace fusion = boost::fusion;

template <int i>
using eq = test::ct_eq<i>;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto eq_lists = make<Tuple>(
          fusion::make_list()
        , fusion::make_list(eq<0>{})
        , fusion::make_list(eq<0>{}, eq<1>{})
        , fusion::make_list(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{})
    );

    auto eq_values = make<Tuple>(eq<0>{}, eq<2>{});

    auto eq_lists_lists = make<Tuple>(
          fusion::make_list()
        , fusion::make_list(
            fusion::make_list(eq<0>{}))
        , fusion::make_list(
            fusion::make_list(eq<0>{}),
            fusion::make_list(eq<1>{}, eq<2>{}))
        , fusion::make_list(
            fusion::make_list(eq<0>{}),
            fusion::make_list(eq<1>{}, eq<2>{}),
            fusion::make_list(eq<3>{}, eq<4>{}))
    );

    //////////////////////////////////////////////////////////////////////////
    // Functor up to Monad
    //////////////////////////////////////////////////////////////////////////
    test::TestFunctor<ext::boost::fusion::List>{eq_lists, eq_values};
    test::TestApplicative<ext::boost::fusion::List>{};
    test::TestMonad<ext::boost::fusion::List>{eq_lists, eq_lists_lists};
}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#pragma once

#include <pcl/kdtree/kdtree.h>
#include <Eigen/StdVector>
#include <stdio.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <stdlib.h>

#include "timer.hpp"
#include "termcolor.hpp"
#include "faster_types.hpp"
// Solvers includes
//#include "solvers/solvers.hpp" CVXGEN solver interface
#include "solverGurobi.hpp"
#include "jps_manager.hpp"

#define MAP 1          // MAP refers to the occupancy grid
#define UNKNOWN_MAP 2  // UNKNOWN_MAP refers to the unkown grid

#define RETURN_LAST_VERTEX 0
#define RETURN_INTERSECTION 1

// status_ : YAWING-->TRAVELING-->GOAL_SEEN-->GOAL_REACHED-->YAWING-->TRAVELING-->...

enum DroneStatus
{
  YAWING = 0,
  TRAVELING = 1,
  GOAL_SEEN = 2,
  GOAL_REACHED = 3
};

enum PlannerStatus
{
  FIRST_PLAN = 0,
  START_REPLANNING = 1,
  REPLANNED = 2
};

using namespace JPS;
using namespace termcolor;

class Faster
{
public:
  Faster(parameters par);
  void replan(vec_Vecf<3>& JPS_safe_out, vec_Vecf<3>& JPS_whole_out, vec_E<Polyhedron<3>>& poly_safe_out,
              vec_E<Polyhedron<3>>& poly_whole_out, std::vector<state>& X_safe_out, std::vector<state>& X_whole_out);
  void updateState(state data);

  void updateMap(pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr_map, pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr_unk);
  bool getNextGoal(state& next_goal);
  void getState(state& data);
  void getG(state& G);
  void setTerminalGoal(state& term_goal);
  void resetInitialization();

private:
  state M_;
  std::deque<state> plan_;

  double previous_yaw_ = 0.0;

  SolverGurobi sg_whole_;  // solver gurobi whole trajectory
  SolverGurobi sg_safe_;   // solver gurobi whole trajectory

  JPS_Manager jps_manager_;  // Manager of JPS

  void yaw(double diff, state& next_goal);

  void getDesiredYaw(state& next_goal);

  // void yaw(double diff, snapstack_msgs::QuadGoal& quad_goal);
  void createMoreVertexes(vec_Vecf<3>& path, double d);

  int findIndexR(int indexH);

  int findIndexH(bool& needToComputeSafePath);
  bool ARisInFreeSpace(int index);

  void updateInitialCond(int i);

  void changeDroneStatus(int new_status);

  Eigen::Vector3d getPos(int i);
  Eigen::Vector3d getVel(int i);

  Eigen::Vector3d getAccel(int i);
  Eigen::Vector3d getJerk(int i);
  // Returns the first collision of JPS with the map (i.e. with the known obstacles). Note that JPS will collide with a
  // map B if JPS was computed using an older map A
  // If type_return==Intersection, it returns the last point in the JPS path that is at least par_.inflation_jps from
  // map
  Eigen::Vector3d getFirstCollisionJPS(vec_Vecf<3>& path, bool* thereIsIntersection, int map, int type_return);

  bool appendToPlan(int k_end_whole, const std::vector<state>& whole, int k_safe, const std::vector<state>& safe);

  bool initialized();
  bool initializedAllExceptPlanner();

  void print_status();

  parameters par_;

  // SeedDecomp3D seed_decomp_util_;

  bool state_initialized_ = false;
  bool planner_initialized_ = false;

  double current_yaw_ = 0;

  double desired_yaw_old_ = 0;

  double alpha_before_ = 0;
  double desired_yaw_B_ = 0;

  vec_E<Polyhedron<3>> polyhedra_;
  std::vector<LinearConstraint3D> l_constraints_whole_;  // Polytope (Linear) constraints
  std::vector<LinearConstraint3D> l_constraints_safe_;   // Polytope (Linear) constraints

  int deltaT_ = 10;
  int deltaT_min_ = 10;
  int indexR_ = 0;

  Eigen::MatrixXd U_safe_, X_safe_;
  double spinup_time_;
  double z_start_;

  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_map_;       // kdtree of the point cloud of the occuppancy grid
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_unk_;       // kdtree of the point cloud of the unknown grid
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_frontier_;  // kdtree of the frontier

  bool kdtree_map_initialized_ = 0;
  bool kdtree_unk_initialized_ = 0;

  bool terminal_goal_initialized_ = false;

  int cells_x_;  // Number of cells of the map in X
  int cells_y_;  // Number of cells of the map in Y
  int cells_z_;  // Number of cells of the map in Z

  int n_states_publised_ = 0;  // Number of goals=states published

  int drone_status_ = DroneStatus::TRAVELING;  // status_ can be TRAVELING, GOAL_SEEN, GOAL_REACHED
  int planner_status_ = PlannerStatus::FIRST_PLAN;

  bool force_reset_to_0_ = 1;

  vec_Vecf<3> JPS_old_;

  double dyaw_filtered_ = 0;

  pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr_map_;
  pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr_unk_;

  std::mutex mtx_map;  // mutex of occupied map (kdtree_map_)
  std::mutex mtx_unk;  // mutex of unkonwn map (pclptr_unk_)
  std::mutex mtx_frontier;
  std::mutex mtx_inst;  // mutex of instanteneous data (v_kdtree_new_pcls_)
  std::mutex mtx_goals;

  std::mutex mtx_k;
  std::mutex mtx_X_U_temp;
  std::mutex mtx_X_U_safe;
  std::mutex mtx_X_U;
  std::mutex mtx_planner_status_;
  std::mutex mtx_initial_cond;
  std::mutex mtx_state;
  std::mutex mtx_offsets;
  std::mutex mtx_plan_;
  // std::mutex mtx_factors;

  std::mutex mtx_G;
  std::mutex mtx_G_term;

  Eigen::Vector3d pos_old_;
  Eigen::Vector3d B_;

  bool to_land_ = false;
  bool JPSk_solved_ = false;

  state stateA_;  // It's the initial condition for the solver
  // flightmode flight_mode_;
  state state_;
  state G_;       // This goal is always inside of the map
  state G_term_;  // This goal is the clicked goal
};