// Disable warnings coming from IfcOpenShell
#pragma warning(disable:4018 4267 4250)

#include <iostream>
#include <ifcparse/Ifc4x3_add2.h>
#include <ifcparse/IfcHierarchyHelper.h>
#include <ifcgeom/abstract_mapping.h>
#include <ifcgeom/iterator.h>
#include <ifcgeom/ifcgeomelement.h>
#include <ifcgeom/kernels/opencascade/OpenCascadeKernel.h>

#include <thread>
#include <ranges>
#include <stack>
#include <vector>
#include <array>
#include <map>
#include <set>
#include <cmath>
#include <numeric>
#include <numbers>
#include <Eigen/Dense>

#define Schema Ifc4x3_add2

template <typename T>
constexpr T deg2rad(T deg) {
   return deg * std::numbers::pi_v<T> / 180.;
}

struct Wire {
   std::vector<Eigen::Vector3d> verts;          // vertex positions
   std::vector<std::array<int, 2>> edges;        // edge list - edge[i] is connected by verts[edge[i][0]] and verts[edge[i][1]]
   std::vector<std::vector<int>> edge_edge;            // edge-edge adjacency - ee[i]

   Wire(const std::vector<Eigen::Vector3d>& v,
      const std::vector<std::array<int, 2>>& e)
      : verts(v), edges(e)
   {
      build_edge_adjacency();
   }

   // ------------------------------------------------------------
   // Build adjacency: ee[i] = list of edges sharing a vertex with edge i
   // ------------------------------------------------------------
   void build_edge_adjacency() {
      std::map<int, std::vector<int>> v2e; // key = vertex index. value = list of edges that connect to that vertex

      // Build vertex → edges map
      for (int ei = 0; ei < edges.size(); ++ei) {
         int a = edges[ei][0];
         int b = edges[ei][1];
         v2e[a].push_back(ei);
         v2e[b].push_back(ei);
      }

      // Build adjacency sets
      std::vector<std::set<int>> neigh(edges.size());
      for (auto& kv : v2e) {
         const auto& es = kv.second; // edges sharing a vertex
         if (es.size() > 1) {
            for (int i : es)
            {
               for (int j : es)
               {
                  if (i != j)
                     neigh[i].insert(j); // edge j is a neighbor of edge i
               }
            }
         }
      }

      // convert to list of adjacent edge indices
      edge_edge.resize(edges.size());
      for (int i = 0; i < neigh.size(); ++i)
         edge_edge[i] = std::vector<int>(neigh[i].begin(), neigh[i].end());
   }

   // ------------------------------------------------------------
   // Decompose wire into smooth-connected components
   // ------------------------------------------------------------
   std::vector<Wire> decompose(double max_edge_angle_deg = 10.0) const {
      double cos_thr = std::cos(deg2rad(max_edge_angle_deg));

      std::vector<bool> used(edges.size(), false);
      std::vector<Wire> components;

      for (int seed = 0; seed < edges.size(); ++seed) {
         if (used[seed]) // if this edge already belongs to a sub-wire, skip it
            continue;

         // seed the stack with an edge, look for all other edges that are approximately in the same direction
         std::stack<int> stack;
         stack.push(seed);
         used[seed] = true;

         std::vector<int> comp; // indices of edges that are part of the current sub-wire

         while (!stack.empty()) {
            int i = stack.top();
            stack.pop();
            comp.push_back(i);

            for (int j : edge_edge[i]) {
               if (used[j])
                  continue; // this edge is already used in a different sub-wire so skip it

               // Compute direction vectors
               Eigen::Vector3d v1 = verts[edges[i][1]] - verts[edges[i][0]];
               Eigen::Vector3d v2 = verts[edges[j][1]] - verts[edges[j][0]];

               double n1 = v1.norm();
               double n2 = v2.norm();
               if (n1 < 1e-12 || n2 < 1e-12)
                  continue;

               v1 /= n1; // normalize vectors
               v2 /= n2;

               if (v1.dot(v2) >= cos_thr) {
                  used[j] = true; // edges hare about in the same direction
                  stack.push(j); // add edge index to the stack so the neighboring edges of this edge get checked next time through the while loop
               }
            }
         }

         // Build sub-wire
         std::vector<std::array<int, 2>> sub_edges;
         sub_edges.reserve(comp.size());
         for (int ei : comp)
            sub_edges.push_back(edges[ei]);

         components.emplace_back(verts, sub_edges); // save the sub-wire
      }

      return components; // return all sub-wires
   }

   // ------------------------------------------------------------
   // Total length of the wire - sum the length of each edge in the wire
   // ------------------------------------------------------------
   double length() const {
      double L = 0.0;
      for (auto& e : edges) {
         Eigen::Vector3d d = verts[e[1]] - verts[e[0]];
         L += d.norm();
      }
      return L;
   }

   // ------------------------------------------------------------
   // Bounding box center
   // ------------------------------------------------------------
   Eigen::Vector3d bbox_center() const {
      if (edges.empty())
         return Eigen::Vector3d::Zero();

      // create bound box around all the edges of the wire
      Eigen::Vector3d vmin = verts[edges[0][0]];
      Eigen::Vector3d vmax = vmin;

      for (auto& e : edges) {
         for (int vi : e) {
            vmin = vmin.cwiseMin(verts[vi]);
            vmax = vmax.cwiseMax(verts[vi]);
         }
      }

      // center point is average value
      return 0.5 * (vmin + vmax);
   }
};

struct Mesh {
   std::vector<Eigen::Vector3d> verts;              // vertex positions
   std::vector<std::array<int, 3>> faces;            // triangle indices
   std::map<std::pair<int, int>, std::vector<int>> edge2faces; // faces sharing an edge
   std::vector<std::vector<int>> face_face;         // face adjacency
   std::vector<Eigen::Vector3d> face_normals;       // per-face normals

   Mesh(const std::vector<Eigen::Vector3d>& v, const std::vector<std::array<int, 3>>& f) :
      verts(v), faces(f)
   {
      build_edge_map();
      build_adjacency();
      compute_face_normals();
   }

   // ------------------------------------------------------------
   // Build edge to face mapping: edge2face[i] = list of faces sharing an edge
   // ------------------------------------------------------------
   void build_edge_map()
   {
      for (int fi = 0; fi < faces.size(); ++fi) {
         auto [a, b, c] = faces[fi]; // the three vertex indices that make up this face

         for (auto [u, v] : { std::pair{a,b}, std::pair{b,c}, std::pair{c,a} }) {
            if (u > v) std::swap(u, v); // order the edges so (7,3) and (3,7) are treated the same
            edge2faces[{u, v}].push_back(fi); // for each edge (u,v) store the face index
         }
      }
   }


   // ------------------------------------------------------------
   // Build adjacency: face_face[i] = list of faces sharing an edge
   // ------------------------------------------------------------
   void build_adjacency() {
      // for each edge shared by multiple faces, the faces are considered to be neighbors
      // if edge (u,v) is used by faces m,n,o, then m, n, o are neighbors
      std::vector<std::set<int>> neigh(faces.size()); // automatic sorting since we are using std::set

      for (auto& kv : edge2faces) {
         const auto& fs = kv.second; // faces sharing an edge
         if (fs.size() > 1) {
            for (int i : fs)
            {
               for (int j : fs)
               {
                  if (i != j)
                     neigh[i].insert(j); // face j is a neighbor of face i
               }
            }
         }
      }

      // convert to list of adjacent face indices
      face_face.resize(faces.size());
      for (int i = 0; i < neigh.size(); ++i)
         face_face[i] = std::vector<int>(neigh[i].begin(), neigh[i].end());
   }

   // ------------------------------------------------------------
   // Compute per-face normals (normalized)
   // ------------------------------------------------------------
   void compute_face_normals() {
      face_normals.resize(faces.size());

      for (int i = 0; i < faces.size(); ++i) {
         auto [a, b, c] = faces[i];
         const Eigen::Vector3d& v0 = verts[a];
         const Eigen::Vector3d& v1 = verts[b];
         const Eigen::Vector3d& v2 = verts[c];

         Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
         double len = n.norm();
         if (len > 1e-12)
            n /= len;

         face_normals[i] = n;
      }
   }

   // ------------------------------------------------------------
   // Decompose into smooth-connected components
   // ------------------------------------------------------------
   std::vector<Mesh> decompose(double max_edge_angle_deg = 10.0) const {
      double cos_thr = std::cos(deg2rad(max_edge_angle_deg));

      std::vector<bool> used(faces.size(), false);
      std::vector<Mesh> components;

      for (int seed = 0; seed < faces.size(); ++seed) {
         if (used[seed]) // if this face already belongs to a sub-mesh, skip it
            continue;

         // seed the stack with a face, look for all other faces that have approximately the same surface normal
         std::stack<int> stack;
         stack.push(seed);
         used[seed] = true;

         std::vector<int> comp; // indices of faces that are part of the current sub-mesh

         while (!stack.empty()) {
            int i = stack.top();
            stack.pop();
            comp.push_back(i);

            const Eigen::Vector3d& ni = face_normals[i]; // surface normal of the curren face

            // check all neighboring faces to see if they have the same surface normal
            for (int j : face_face[i]) {
               if (used[j])
                  continue; // this face is already used in a different sub-mesh so skip it

               if (ni.dot(face_normals[j]) >= cos_thr) {
                  used[j] = true; // faces have about the same surface normal
                  stack.push(j); // add face index to the stack so the neighboring faces of this face get checked next time through the while loop
               }
            }
         }

         // Build sub-mesh
         std::vector<std::array<int, 3>> sub_faces; // vector of faces, defined by their vertex indices, which all have about the same surface normal
         sub_faces.reserve(comp.size());
         for (int fi : comp)
            sub_faces.push_back(faces[fi]);

         components.emplace_back(verts, sub_faces); // save the sub-mesh
      }

      return components; // return all sub-meshes
   }

   // ------------------------------------------------------------
   // Boundary extraction (outer boundary only)
   // ------------------------------------------------------------
   Wire boundary() const {
      // Collect boundary edges (edges used by exactly 1 face)
      std::vector<std::array<int, 2>> boundary_edges; // bound_edges[i] -> start vertex index, end vertex index
      for (auto& kv : edge2faces) {
         if (kv.second.size() == 1) {
            boundary_edges.push_back({ kv.first.first, kv.first.second });
         }
      }

      // Build a graph to order boundary edges into a cycle
      // (Assumes a single outer boundary)
      std::map<int, std::vector<int>> graph;
      for (auto& e : boundary_edges) {
         graph[e[0]].push_back(e[1]);
         graph[e[1]].push_back(e[0]);
      }

      // Find cycle
      std::vector<int> cycle;
      int start = boundary_edges[0][0];
      int prev = -1;
      int cur = start;

      do {
         cycle.push_back(cur);
         const auto& nbrs = graph[cur];
         int next = (nbrs[0] == prev ? nbrs[1] : nbrs[0]);
         prev = cur;
         cur = next;
      } while (cur != start);

      // Convert cycle to edges
      std::vector<std::array<int, 2>> ordered_edges;
      for (int i = 0; i < cycle.size(); ++i) {
         int a = cycle[i];
         int b = cycle[(i + 1) % cycle.size()];
         ordered_edges.push_back({ a, b });
      }

      return Wire(verts, ordered_edges);
   }
};

int main()
{
   Logger::SetOutput(&std::cout, &std::cout);

   IfcParse::IfcFile file("C:\\Users\\bricer\\OneDrive - Washington State Department of Transportation\\Desktop\\PGSuper_Import_Model.ifc");
   if (!file.good()) {
	  std::cout << "Unable to parse .ifc file" << std::endl;
	  return 1;
   }

   ifcopenshell::geometry::Settings settings;
   settings.set("use-world-coords", true);
   settings.set("weld-vertices", true);
   settings.set("disable-opening-subtractions", true);

   auto beams = file.instances_by_type<Schema::IfcBeam>();

   // get the ids of all the beams whose name starts with "Span 1"
   std::set<int> beam_ids;
   for (int id : *beams 
      | std::views::filter([](auto beam) {return beam->Name() && beam->Name()->starts_with("Span 1"); }) // filter all beams that start with "Span 1"
      | std::views::transform([](auto beam) {return beam->id(); })) // transform the beam to its id
   {
      beam_ids.insert(id);
   }

   IfcGeom::instance_id_filter filter(true, false, beam_ids);
   std::vector<IfcGeom::filter_t> filters({ filter });

   std::unique_ptr<IfcGeom::OpenCascadeKernel> kernel(std::make_unique<IfcGeom::OpenCascadeKernel>(settings));
   int num_threads = std::thread::hardware_concurrency();
   IfcGeom::Iterator iterator(std::move(kernel), settings, &file, filters, num_threads);

   bool bResult = iterator.initialize();
   do
   {
      auto element = iterator.get();
      std::cout << element->name() << std::endl;;

      auto triangulation = dynamic_cast<IfcGeom::TriangulationElement*>(element);
      auto geometry = triangulation->geometry_pointer();
      const auto& verts = geometry->verts();
      const auto& faces = geometry->faces();

      std::vector<Eigen::Vector3d> v;
      v.reserve(verts.size() / 3);
      for (auto i = 0; i < verts.size(); i += 3)
      {
         v.emplace_back(verts[i], verts[i + 1], verts[i + 2]);
      }

      std::vector<std::array<int, 3>> f;
      f.reserve(faces.size() / 3);
      for (auto i = 0; i < faces.size(); i += 3)
      {
         f.emplace_back(std::array<int, 3>({ faces[i],faces[i + 1],faces[i + 2] }));
      }

      // 1. Build mesh
      Mesh m(v, f);

      // 2. Decompose mesh into smooth components
      std::vector<Mesh> decomposed = m.decompose();

      // 3. Select top component (max Z of vertex + avg normal)
      auto top_component_it = std::max_element(
         decomposed.begin(), decomposed.end(),
         [](const Mesh& a, const Mesh& b)
         {
            auto score = [](const Mesh& c)
               {
                  // Compute average normal
                  Eigen::Vector3d avg = Eigen::Vector3d::Zero();
                  for (auto& n : c.face_normals)
                     avg += n;

                  avg /= (double)c.face_normals.size();

                  // Compute max Z of (vertex + avg_normal)
                  double best = -1e18;
                  for (auto& f : c.faces) {
                     for (int vi : f) {
                        double z = (c.verts[vi] + avg).z();
                        if (z > best)
                           best = z;
                     }
                  }
                  return best;
               };

            return score(a) < score(b);
         }
      );

      Mesh top_component = *top_component_it;

      // 4. Extract boundary wire
      Wire boundary = top_component.boundary();

      // 5. Decompose boundary into smooth segments
      std::vector<Wire> segments = boundary.decompose();

      // 6. Sort segments by length
      std::sort(segments.begin(), segments.end(),
         [](const Wire& a, const Wire& b)
         {
            return a.length() < b.length();
         });

      // 7. Take the two shortest segments
      Wire start_seg = segments[0];
      Wire end_seg = segments[1];

      // 8. Print bounding-box centers
      Eigen::Vector3d c1 = start_seg.bbox_center();
      Eigen::Vector3d c2 = end_seg.bbox_center();

      std::cout << c1.transpose() << " -> " << c2.transpose() << "\n";


   } while (iterator.next());
}