// ========================================================================== //
// Class inclusion
#include "Pointcloud.hpp"

// Local libraries
#include "Utilities/string_operations.hpp"

// External libraries
#include <GEL/CGLA/CGLA.h>

// Standard libraries
#include <fstream>
#include <sstream>
#include <string>
#include <vector>

// ========================================================================== //
// Implementations

PointCloud::PointCloud(int N) {
    point_list  = std::vector<CGLA::Vec3d>(N);
    normal_list = std::vector<CGLA::Vec3d>(N);
    color_list  = std::vector<double>(N);
}
PointCloud::PointCloud(CGLA::Vec3d p, CGLA::Vec3d n, double c) {
    point_list = {p}, normal_list = {n}, color_list = {c};
}
PointCloud::PointCloud(std::vector<CGLA::Vec3d> points,
                       std::vector<CGLA::Vec3d> normals,
                       std::vector<double>      layers) {
    point_list = points, normal_list = normals, color_list = layers;
}

// Data access
CGLA::Vec3d &PointCloud::points(int idx) { return point_list[idx]; }
CGLA::Vec3d &PointCloud::normals(int idx) { return normal_list[idx]; }
double &     PointCloud::colors(int idx) { return color_list[idx]; }

std::vector<CGLA::Vec3d> &PointCloud::points() { return point_list; }
std::vector<CGLA::Vec3d> &PointCloud::normals() { return normal_list; }
std::vector<double> &     PointCloud::colors() { return color_list; }

size_t PointCloud::size() { return point_list.size(); }

// Combine pointclouds
void PointCloud::push_back(PointCloud &pcl) {
    for (auto p : pcl.point_list) point_list.push_back(p);
    for (auto n : pcl.normal_list) normal_list.push_back(n);
    for (auto l : pcl.color_list) color_list.push_back(l);
}
// Add new point at the end of the lists
void PointCloud::push_back(CGLA::Vec3d p, CGLA::Vec3d n, double l) {
    point_list.push_back(p);
    normal_list.push_back(n);
    color_list.push_back(l);
}

// Function used to export pointclouds as .ply files
void ply_writer(std::string fileName, PointCloud &pcl) {

    if (pcl.size() < 1) return;

    // https://en.wikipedia.org/wiki/PLY_(file_format)

    std::ofstream file;
    file.open(fileName, std::ios::out);
    if (!file.good())
        Utilities::error(__FILE__, __LINE__,
                         "ply_writer: error opening file, " + fileName);

    file << "ply"
         << "\n";
    file << "format ascii 1.0"
         << "\n";
    file << "element vertex " << pcl.size() << "\n";
    file << "property float x"
         << "\n";
    file << "property float y"
         << "\n";
    file << "property float z"
         << "\n";
    file << "property float nx"
         << "\n";
    file << "property float ny"
         << "\n";
    file << "property float nz"
         << "\n";
    file << "property uchar red"
         << "\n";
    file << "property uchar green"
         << "\n";
    file << "property uchar blue"
         << "\n";
    file << "end_header"
         << "\n";

    size_t N = pcl.size();

    for (size_t i = 0; i < N; i++) {
        CGLA::Vec3d p = pcl.point_list[i];
        CGLA::Vec3d n = pcl.normal_list[i];
        int         c = 255;
        if (pcl.color_list.size() != 0) c = int(255.0 * pcl.color_list[i]);
        file << p[0] << " " << p[1] << " " << p[2] << " ";
        file << n[0] << " " << n[1] << " " << n[2] << " ";
        file << c << " " << c << " " << c << "\n";
    }

    file.close();
}

// Function used to import pointclouds as .ply files
void ply_reader(std::string fileName, PointCloud &pcl) {

    // https://en.wikipedia.org/wiki/PLY_(file_format)

    // Assumptions:
    // - file

    std::ifstream file(fileName);
    if (!file.good())
        Utilities::error(__FILE__, __LINE__, "Error importing ply file.");

    // File reading variables
    std::string              line, cell;
    std::stringstream        lineStream(line);
    std::vector<double>      values;
    std::vector<std::string> header;

    // Meta information
    std::vector<std::string> element_types;
    std::vector<int>         element_amount;

    getline(file, line);
    if (line.compare("ply") != 0)
        Utilities::error(__FILE__, __LINE__,
                         "Imported file is not a ply file.");

    // Read the header for information on file contents
    while (line.compare("end_header") != 0) {

        // File inputs
        getline(file, line);
        lineStream = std::stringstream(line);
        header.clear();
        while (getline(lineStream, cell, ' ')) header.push_back(cell);

        // Read elements
        if (header.size() != 3) continue;
        if (header[0].compare("element") == 0 &&
            header[1].compare("vertex") == 0) {
            element_types.push_back("vertex");
            element_amount.push_back(stoi(header[2]));
        }
    }

    for (size_t e = 0; e < element_types.size(); e++) {
        std::string e_type   = element_types[e];
        size_t      e_amount = element_amount[e];

        // Read in the vertex elements
        if (e_type.compare("vertex") == 0) {

            // Prepare variables
            std::vector<CGLA::Vec3d> pointcloud(e_amount);
            std::vector<CGLA::Vec3d> normals(e_amount);
            std::vector<double>      layers(e_amount);

            // Read in the information from the file
            for (size_t vertex_nr = 0; vertex_nr < e_amount; vertex_nr++) {

                // Get values from file
                getline(file, line);
                lineStream = std::stringstream(line);
                values.clear();
                while (getline(lineStream, cell, ' ')) {
                    try {
                        values.push_back(stod(cell));
                    } catch (const std::out_of_range &oor) {
                        values.push_back(0.0);
                    }
                }

                // Read vertex positions
                if (values.size() < 3) continue;
                pointcloud[vertex_nr] =
                    CGLA::Vec3d(values[0], values[1], values[2]);

                // Read vertex normals
                if (values.size() < 6) continue;
                normals[vertex_nr] =
                    CGLA::Vec3d(values[3], values[4], values[5]);

                if (values.size() < 7) continue;
                layers[vertex_nr] = values[6] / 255.0;
            }

            // Save the information to the class
            pcl.point_list  = pointcloud;
            pcl.normal_list = normals;
            pcl.color_list  = layers;
        }

        // Skip through unsupported lines
        else {
            for (size_t tmp = 0; tmp < e_amount; tmp++) getline(file, line);
        }
    }

    file.close();
}