// ========================================================================== //
// Class inclusion
#include "Frames/FrameField.hpp"

// Local libraries
#include "Matrix/Matrix.hpp"
#include "Pointcloud/Pointcloud.hpp"
#include "Utilities/math_operations.hpp"
#include "Utilities/string_operations.hpp"

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

// Standard libraries
#include <filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <string>
#include <vector>

// Parallellisation
#include <omp.h>
#if USE_MPI == 1
    #include <mpi.h>
#endif

// Macros
#ifndef MPI_VERSION
    #define MPI_VERSION 0
#endif

// ========================================================================== //
// Constructor and IO

FrameGrid::FrameGrid() {}
FrameGrid::FrameGrid(int X, int Y, int Z, int F) {
    dimX = X, dimY = Y, dimZ = Z, dimF = F;
    element_size = 1.0;

    bbox.extend(CGLA::Vec3d(0.0));              // Set the lower bounds
    bbox.extend(CGLA::Vec3d(dimX, dimY, dimZ)); // Set the upper bound

    numel = X * Y * Z;

    frame_grid = Matrix<size_t>(dimX, dimY, dimZ);

    // Allocate data arrays
    pos_list   = std::vector<CGLA::Vec3d>(numel);
    frame_list = std::vector<std::vector<CGLA::Vec3d>>(
        numel, std::vector<CGLA::Vec3d>(dimF));
    layer_list =
        std::vector<std::vector<double>>(numel, std::vector<double>(dimF));
    densi_list = std::vector<double>(numel);

    // Build the indexing grid
    for (size_t i = 0; i < numel; i++) frame_grid.set(i, i);
}

void FrameGrid::read(std::string data_location) {

    // Open the file
    std::ifstream dataFile(data_location + "field.csv");
    if (!dataFile.good())
        Utilities::error(__FILE__, __LINE__, "Data File Error: \n\tfield.csv.");

    // Layer thickness
    std::ifstream layerFile(data_location + "width.csv");
    if (!layerFile.good())
        Utilities::error(__FILE__, __LINE__,
                         "Layer File Error: \n\t" + data_location +
                             "width.csv" + ".");

    // Read the file and fill up the temporary vectors
    std::string frame_line, layer_line;
    while (std::getline(dataFile, frame_line) &&
           std::getline(layerFile, layer_line)) {

        // Reading the frame file
        std::stringstream lineStream(frame_line), layer_lineStream(layer_line);
        std::string       frame_cell, layer_cell;
        std::vector<std::string> frame_values, layer_values;

        while (std::getline(lineStream, frame_cell, ','))
            frame_values.push_back(frame_cell);

        // Read in the layer
        while (std::getline(layer_lineStream, layer_cell, ','))
            layer_values.push_back(layer_cell);

        // Add values to the variables
        std::vector<int> idx = {std::stoi(frame_values[0]) - 1,
                                std::stoi(frame_values[1]) - 1,
                                std::stoi(frame_values[2]) - 1};

        CGLA::Vec3d pos = {std::stod(frame_values[0]) - 0.5,
                           std::stod(frame_values[1]) - 0.5,
                           std::stod(frame_values[2]) - 0.5};

        // Read frame directions
        std::vector<CGLA::Vec3d> frame;
        for (size_t i = 5; i < frame_values.size(); i += 3)
            frame.push_back({std::stod(frame_values[i - 2]),
                             std::stod(frame_values[i - 1]),
                             std::stod(frame_values[i])});

        std::vector<double> layer;
        for (std::string l : layer_values) layer.push_back(std::stod(l));

        // Compute the density
        double density = 1.0;
        for (double l : layer) density *= 1.0 - l;
        density = 1.0 - density;

        for (size_t i = 0; i < frame.size(); i++)
            frame[i] = CGLA::cond_normalize(frame[i]);

        // Save to class
        int N         = frame_grid.get(idx);
        pos_list[N]   = pos;
        frame_list[N] = frame;
        layer_list[N] = layer;
        densi_list[N] = density;
    }
    dataFile.close();
    layerFile.close();
}

void FrameGrid::export_singularity(std::string export_location) {
#if MPI_VERSION > 0
    int rank;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    if (rank != 0) return;
#endif
    // Write singularity structure to file
    std::string fileName = export_location + "singularity.ply";
    Utilities::valid_directory(export_location);

    PointCloud pcl;
    for (size_t id : singular_elements) pcl.push_back(pos_list[id]);

    ply_writer(fileName, pcl);
}

// ========================================================================== //
// Lookup index of enclosing element

int FrameGrid::get_field_index(const CGLA::Vec3d &p) {

    int i = p[0] - 0.5, j = p[1] - 0.5, k = p[2] - 0.5;
    if (i >= 0 && j >= 0 && k >= 0 && i < dimX && j < dimY && k < dimZ)
        return frame_grid.get(i, j, k);
    return -1;
}

// ========================================================================== //
// Interpolation functions

std::vector<double> FrameGrid::get_local(const CGLA::Vec3d &p) {
    CGLA::Vec3d p_i = lookup_position(p);
    CGLA::Vec3d xyz =
        CGLA::v_min(CGLA::v_max(p - p_i, CGLA::Vec3d(0.0)), CGLA::Vec3d(1.0));

    return std::vector<double>({xyz[0], xyz[1], xyz[2]});
}

// Find the 4 nearest gridpoints
std::vector<size_t> FrameGrid::neighbours(const CGLA::Vec3d &p) {

    int i0 = std::max(std::min((int)(p[0] - 0.5), dimX - 1), 0);
    int j0 = std::max(std::min((int)(p[1] - 0.5), dimY - 1), 0);
    int k0 = std::max(std::min((int)(p[2] - 0.5), dimZ - 1), 0);
    int i1 = std::min(i0 + 1, dimX - 1);
    int j1 = std::min(j0 + 1, dimY - 1);
    int k1 = std::min(k0 + 1, dimZ - 1);

    std::vector<size_t> corners = {
        frame_grid.get(i0, j0, k0), frame_grid.get(i1, j0, k0),
        frame_grid.get(i0, j1, k0), frame_grid.get(i1, j1, k0),
        frame_grid.get(i0, j0, k1), frame_grid.get(i1, j0, k1),
        frame_grid.get(i0, j1, k1), frame_grid.get(i1, j1, k1)};

    return corners;
}
double FrameGrid::interpolate(const std::vector<double> &local,
                              const std::vector<double> &value) {
    CGLA::Vec3d xyz(local[0], local[1], local[2]);
    return Utilities::interpolate_trilinear(xyz, value);
}
CGLA::Vec3d FrameGrid::interpolate(const std::vector<double> &     local,
                                   const std::vector<CGLA::Vec3d> &value) {
    CGLA::Vec3d xyz(local[0], local[1], local[2]);
    return Utilities::interpolate_trilinear(xyz, value);
}

// ========================================================================== //
// Interpolation functions

void FrameGrid::detect_singular(std::string filename) {
    // Function designed to locate and block singularities in the fields.
    // We assume 3 directions for now.
    // Computation based on work by Groen and Stutz (2019 arXiv:1910.13002v1)
    // section 3.2.

    /*
        double      threshold                 = 0.1;
        int         singularity_neighbourhood = 3;
        std::string export_location           = "result/";

        // Local variables
        std::vector<std::vector<double>> result(numel,
                                                std::vector<double>(dimF, 0.0));

        int id_0 = 0, id_N = numel;
        for (int idx = id_0; idx < id_N; idx++) {

            // Construct ijk from element idx
            std::vector<int> ijk = {idx % (dimX), idx / (dimX) % (dimY),
                                    idx / ((dimX) * (dimY)) % (dimZ)};

            int I = frame_grid.get(ijk);

            // Setup variables
            std::vector<std::vector<int>> neigh_list =
                neighbours(ijk, singularity_neighbourhood);

            std::vector<CGLA::Vec3d> field = this->lookup_frame(I);

            // Compute rotation needed to align neigh and center frames
            std::vector<std::vector<double>> angles;

            for (std::vector<int> neigh : neigh_list) {
                int I_neigh = frame_grid.get(neigh);

                std::vector<CGLA::Vec3d> field_neigh =
       this->lookup_frame(I_neigh);

                // Setup list of right handed frames for the neighbour
                std::vector<double> val(field.size(), 0.0);

                for (size_t s = 0; s < field.size(); s++) {
                    CGLA::Vec3d p_n = pos_list[I_neigh];
                    // Setup the frame
                    CGLA::Vec3d v1 = field[s];
                    CGLA::Vec3d v2 = lookup_frame_nearest(p_n, v1);

                    if (CGLA::length(v1) < 1.0 || CGLA::length(v2) < 1.0)
                        continue;

                    // Save value if smaller
                    val[s] = 1.0 - std::abs(CGLA::dot(v1, v2));
                }
                angles.push_back(val);
            }
            if (angles.size() == 0)
                continue;

            // Compute average divergence value.
            std::vector<double> divergence_value(3, 0.0);
            for (std::vector<double> v : angles) {
                divergence_value[0] += v[0];
                divergence_value[1] += v[1];
                divergence_value[2] += v[2];
            }
            divergence_value[0] /= (double)angles.size();
            divergence_value[1] /= (double)angles.size();
            divergence_value[2] /= (double)angles.size();

            result[idx] = divergence_value;
        }

        double minimal = 1e-6; // Ignore values smaller than the minimal
        double c       = minimal;

        // Compute statistics
        double tmp_numel = 3.0 * numel; // Number of used values
        for (size_t s : {0, 1, 2}) {
            for (size_t i = 0; i < result.size(); i++)
                tmp_numel -= result[i][s] < minimal ? 1.0 : 0.0;
        }

        // Estimate mean value
        double mu = 0.0;
        for (size_t s : {0, 1, 2}) {
            for (size_t i = 0; i < result.size(); i++)
                mu += result[i][s] < minimal ? 0.0 : result[i][s];
        }
        mu /= tmp_numel;

        // Estimate variance and STD
        double s2 = 0.0;
        for (size_t s : {0, 1, 2}) {
            for (size_t i = 0; i < result.size(); i++)
                s2 +=
                    result[i][s] < minimal ? 0.0 : std::pow(result[i][s] -
       mu, 2.0);
        }
        s2 /= tmp_numel;
        double s1 = std::sqrt(s2);

        // Compute cut-off value
        c = mu + threshold * s1;

        // Supress the locations we dont need
        for (int idx = 0; idx < (int)numel; idx++) {
            for (int s = 0; s < dimF; s++) {
                if (result[idx][s] < c)
                    continue;

                std::vector<int> ijk = {idx % (dimX), idx / (dimX) % (dimY),
                                        idx / ((dimX) * (dimY)) % (dimZ)};

                int I           = frame_grid.get(ijk);
                singulars[I][s] = true;
            }
        }
        */
}

// End of File
// ========================================================================== //