#pragma once
#include "common.h"

void noise_removal(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    if(cloud->size() == 0)
    {
        std::cout << "[Error] pointcloud empty" << std::endl;
        return;
    }

    const double r = 0.1;
    const int min_neighbor = 50;

    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    outrem.setInputCloud(cloud);
    outrem.setRadiusSearch(r);
    outrem.setMinNeighborsInRadius(min_neighbor);
    outrem.filter(*cloud);

    std::cout << "pointcloud filtered..." << std::endl;
}

void downsampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    if(cloud->size() == 0)
    {
        std::cout << "[Error] pointcloud empty" << std::endl;
        return;
    }

    const double leaf = 0.05;

    pcl::VoxelGrid<pcl::PointXYZ> grid;
    grid.setLeafSize(leaf, leaf, leaf);
    grid.setInputCloud(cloud);
    grid.filter(*cloud);
    
    std::cout << "pointcloud downsampled..." << std::endl;
}

void upsampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    //mls upsampling
    pcl::PointCloud<pcl::PointNormal>::Ptr tmp(new pcl::PointCloud<pcl::PointNormal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;

    mls.setComputeNormals(true);

    mls.setInputCloud(cloud);
    mls.setPolynomialOrder(2);
    mls.setSearchMethod(tree);
    mls.setSearchRadius(0.03);

    mls.process(*tmp);

    pcl::PointCloud<pcl::PointXYZ>::Ptr filter(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->clear();
    pcl::copyPointCloud(*tmp, *filter);

    std::vector<int> idx;
    pcl::removeNaNFromPointCloud(*filter, *cloud, idx); //not working
}

void normal_estimation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals)
{
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

    tree->setInputCloud(cloud);
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(30);
    n.compute(*normals);

    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
}

void triangulation(pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals, pcl::PolygonMesh& triangles)
{
    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp;
    
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree_normal(new pcl::search::KdTree<pcl::PointNormal>);
    tree_normal->setInputCloud(cloud_with_normals);

    gp.setSearchRadius(0.025);
    gp.setMu(2.5);
    gp.setMaximumNearestNeighbors(200);
    gp.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
    gp.setMaximumAngle(2*M_PI/3); // 120 degrees
    gp.setMinimumAngle(M_PI/18); // 10 degrees
    gp.setNormalConsistency(false);
    gp.setConsistentVertexOrdering(true);
    gp.setInputCloud(cloud_with_normals);
    gp.setSearchMethod(tree_normal);
    gp.reconstruct(triangles);
}

int saveOBJFile (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, camInfo cam)
{
  if (tex_mesh.cloud.data.empty ())
  {
    PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no data!\n");
    return (-1);
  }

  //Open file
  std::ofstream fs;
  fs.precision(precision);
  fs.open(file_name.c_str());

  //Define material file
  std::string mtl_file_name = file_name.substr(0, file_name.find_last_of ('.')) + ".mtl";
  //Strip path for "mtllib" command
  std::string mtl_file_name_nopath = mtl_file_name;
  mtl_file_name_nopath.erase(0, mtl_file_name.find_last_of ('/') + 1);

  /* Write 3D information */
  //number of points
  int nr_points  = tex_mesh.cloud.width * tex_mesh.cloud.height;
  int point_size = tex_mesh.cloud.data.size () / nr_points;

  //number of meshes
  int nr_meshes = tex_mesh.tex_polygons.size ();

  //number of faces for header
  int nr_faces = 0;
  for (int m = 0; m < nr_meshes; ++m)
    nr_faces += tex_mesh.tex_polygons[m].size ();

  // Write the header information
  fs << "####" << std::endl;
  fs << "# OBJ dataFile simple version. File name: " << file_name << std::endl;
  fs << "# Vertices: " << nr_points << std::endl;
  fs << "# Faces: " << nr_faces << std::endl;
  fs << "# Material information:" << std::endl;
  fs << "mtllib " << mtl_file_name_nopath << std::endl;
  fs << "####" << std::endl;

  // Write vertex coordinates
  PCL_INFO ("Writing vertices...\n");
  fs << "# Vertices" << std::endl;
  for (int i = 0; i < nr_points; ++i)
  {
    int xyz = 0;
    // "v" just be written one
    bool v_written = false;
    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
    {
      // adding vertex
      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
                tex_mesh.cloud.fields[d].name == "x" ||
                tex_mesh.cloud.fields[d].name == "y" ||
                tex_mesh.cloud.fields[d].name == "z"))
      {
        if (!v_written)
        {
            // write vertices beginning with v
            fs << "v ";
            v_written = true;
        }
        float value;
        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
        fs << value;
        if (++xyz == 3)
            break;
        fs << " ";
      }
    }
    if (xyz != 3)
    {
      PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no XYZ data!\n");
      return (-2);
    }
    fs << std::endl;
  }
  fs << "# "<< nr_points <<" vertices" << std::endl;

  // Write vertex texture
  PCL_INFO ("Writing textures...\n");
  fs << "# " << cloud->size() << " vertex textures in mesh " << std::endl;
  for (int i = 0; i < cloud->size(); ++i)
  {
    auto pt = cloud->at(i);
    fs << "vt ";
    fs << (pt.x * cam.fx / pt.z + cam.cx) / cam.width << " ";
    fs << 1.0 - ((pt.y * cam.fy / pt.z + cam.cy) / cam.height) << std::endl;
  }

  // Write vertex normals
  PCL_INFO ("Writing normals...\n");
  for (int i = 0; i < nr_points; ++i)
  {
    int xyz = 0;
    // "vn" just be written one
    bool v_written = false;
    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
    {
      // adding vertex
      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
      tex_mesh.cloud.fields[d].name == "normal_x" ||
      tex_mesh.cloud.fields[d].name == "normal_y" ||
      tex_mesh.cloud.fields[d].name == "normal_z"))
      {
        if (!v_written)
        {
          // write vertices beginning with vn
          fs << "vn ";
          v_written = true;
        }
        float value;
        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
        fs << value;
        if (++xyz == 3)
          break;
        fs << " ";
      }
    }
    if (xyz != 3)
    {
    PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no normals!\n");
    return (-2);
    }
    fs << std::endl;
  }

  PCL_INFO ("Writing faces...\n");
  int f_idx = 0;
  for (int m = 0; m < nr_meshes; ++m)
  {
    if (m > 0) 
      f_idx += tex_mesh.tex_polygons[m-1].size ();

    if(!tex_mesh.tex_materials.empty ())
    {
      fs << "# The material will be used for mesh " << m << std::endl;
      //TODO pbl here with multi texture and unseen faces
      fs << "usemtl " <<  tex_mesh.tex_materials[m].tex_name << std::endl;
      fs << "# Faces" << std::endl;
    }
    for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i)
    {
      // Write faces with "f"
      fs << "f";
      // There's one UV per vertex per face, i.e., the same vertex can have
      // different UV depending on the face.
      for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
      {
        unsigned int idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1;
        fs << " " << idx
        << "/" << idx
        << "/" << idx; // vertex index in obj file format starting with 1
      }
      fs << std::endl;
    }
    //PCL_INFO ("%d faces in mesh %d \n", tex_mesh.tex_polygons[m].size () , m);
    fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << std::endl;
  }
  fs << "# End of File";

  // Close obj file
  PCL_INFO ("Closing obj file\n");
  fs.close ();

  /* Write material definition for OBJ file*/
  // Open file
  PCL_INFO ("Writing material files\n");
  //don't do it if no material to write
  if(tex_mesh.tex_materials.empty ())
    return (0);

  std::ofstream m_fs;
  m_fs.precision(precision);
  m_fs.open(mtl_file_name.c_str ());

  //default
  m_fs << "#" << std::endl;
  m_fs << "# Wavefront material file" << std::endl;
  m_fs << "#" << std::endl;
  for(int m = 0; m < nr_meshes; ++m)
  {
    m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << std::endl;
    m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << std::endl; // defines the ambient color of the material to be (r,g,b).
    m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << std::endl; // defines the diffuse color of the material to be (r,g,b).
    m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << std::endl; // defines the specular color of the material to be (r,g,b). This color shows up in highlights.
    m_fs << "d " << tex_mesh.tex_materials[m].tex_d << std::endl; // defines the transparency of the material to be alpha.
    m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns  << std::endl; // defines the shininess of the material to be s.
    m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << std::endl; // denotes the illumination model used by the material.
    // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used.
    // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required.
    m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << std::endl;
    m_fs << "###" << std::endl;
  }
  m_fs.close ();
  return (0);
}