// ========================================================================== //
// 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 <string>
#include <vector>

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

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

// ========================================================================== //
// Forward declarations

void FrameField::read(std::string) {
    Utilities::error(__FILE__, __LINE__, "Function: 'read' not implemented");
}
int FrameField::get_field_index(const CGLA::Vec3d &) {
    Utilities::error(__FILE__, __LINE__,
                     "Function: 'get_field_index' not implemented");
    return -1;
}
void FrameField::detect_singular(std::string) {
    Utilities::warning(__FILE__, __LINE__,
                       "Function: 'detect_singular' not implemented");
}
std::vector<size_t> FrameField::neighbours(const CGLA::Vec3d &) {
    Utilities::error(__FILE__, __LINE__,
                     "Function: 'neighbours' not implemented");
    return std::vector<size_t>();
}
std::vector<double> FrameField::get_local(const CGLA::Vec3d &) {
    Utilities::error(__FILE__, __LINE__,
                     "Function: 'get_local' not implemented");
    return std::vector<double>();
}
double FrameField::interpolate(const std::vector<double> &local,
                               const std::vector<double> &value) {
    Utilities::error(__FILE__, __LINE__,
                     "Function: 'interpolate' not implemented");
    return 0.0;
}
CGLA::Vec3d FrameField::interpolate(const std::vector<double> &     local,
                                    const std::vector<CGLA::Vec3d> &value) {
    Utilities::error(__FILE__, __LINE__,
                     "Function: 'interpolate' not implemented");
    return CGLA::Vec3d();
}
// Interpolate weighted frame at point.
std::vector<CGLA::Vec3d> FrameField::interpolate_frame(const CGLA::Vec3d &p) {
    // This function interpolates the stress direction field at a given point,
    // the point will be a vertex, halfedge center or point.

    // Extract corners
    std::vector<size_t> nodes = this->neighbours(p);
    if (nodes.size() == 0)
        return std::vector<CGLA::Vec3d>(dimF, CGLA::Vec3d(0.0));

    // Align the indecies of corners.
    std::vector<std::vector<int>> alignment = align_index(nodes);

    // Fill the data
    std::vector<std::vector<CGLA::Vec3d>> data(
        dimF, std::vector<CGLA::Vec3d>(nodes.size()));
    for (size_t i = 0; i < nodes.size(); i++) {
        int                      idx   = nodes[i];
        std::vector<CGLA::Vec3d> frame = this->lookup_frame(idx);
        for (int s = 0; s < dimF; s++) data[s][i] = frame[alignment[i][s]];
    }

    // Computing barycentric interpolation of directions from grid
    std::vector<double> local = get_local(p);

    std::vector<CGLA::Vec3d> result(dimF);
    for (int s_id = 0; s_id < dimF; s_id++)
        result[s_id] = CGLA::cond_normalize(interpolate(local, data[s_id]));

    return result;
}
// Interpolate weighted layer at point.
std::vector<double> FrameField::interpolate_layer(const CGLA::Vec3d &p) {
    // This function interpolates the stress direction field at a given point,
    // the point will be a vertex, halfedge center or point.

    // Extract corners
    std::vector<size_t> nodes = this->neighbours(p);
    if (nodes.size() < 1) return std::vector<double>(dimF, 0.0);

    // Align the indecies of corners.
    std::vector<std::vector<int>> alignment = align_index(nodes);

    // Fill the data
    std::vector<std::vector<double>> data(dimF,
                                          std::vector<double>(nodes.size()));
    for (size_t i = 0; i < nodes.size(); i++) {
        int                 idx   = nodes[i];
        std::vector<double> layer = this->lookup_layer(idx);
        for (int s = 0; s < dimF; s++) data[s][i] = layer[alignment[i][s]];
    }

    // Computing bi-linear interpolation of directions from grid
    std::vector<double> local = get_local(p);

    std::vector<double> result(dimF);
    for (int s_id = 0; s_id < dimF; s_id++)
        result[s_id] = interpolate(local, data[s_id]);

    return result;
}
// Interpolate weighted density at point.
double FrameField::interpolate_density(const CGLA::Vec3d &p) {

    // Extract corners
    std::vector<size_t> nodes = this->neighbours(p);
    if (nodes.size() < 1) return 0.0;

    // Fill the data
    std::vector<double> data(nodes.size());
    for (size_t i = 0; i < nodes.size(); i++)
        data[i] = lookup_density(nodes[i]);

    // Computing bi-linear interpolation of directions from grid
    std::vector<double> local = get_local(p);

    return interpolate(local, data);
}
// ========================================================================== //
// Constructors and IO

FrameField::FrameField(){};

void FrameField::read_singular(std::string data_location) {

    // Open the file
    std::ifstream dataFile(data_location + "singular.csv");
    if (!dataFile.good())
        return detect_singular(data_location + "singular.csv");

    // Read the file and fill up the temporary vectors
    std::string frame_line;
    while (std::getline(dataFile, frame_line))
        singular_elements.push_back(std::stoi(frame_line));

    dataFile.close();
}

void FrameField::export_densities(std::string export_location) {

    std::string fileName = export_location + "densities.csv";
    Utilities::valid_directory(export_location);

#if MPI_VERSION > 0
    int rank;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    if (rank != 0) return;
#endif

    // Write density values to file
    std::ofstream file;
    file.open(fileName, std::ios::out);

    for (size_t idx = 0; idx <= numel; idx++) {
        CGLA::Vec3d p = pos_list[idx];
        double      d = densi_list[idx];
        file << p[0] << "," << p[1] << "," << p[2] << "," << d << "\n";
    }

    file.close();
}

void FrameField::export_singularity(std::string export_location) {
    Utilities::warning(__FILE__, __LINE__,
                       "Function: 'export_singularity' not implemented");
}

// ========================================================================== //
// Operations to get frame at a point

// Lookup frame at a point.
std::vector<CGLA::Vec3d> FrameField::lookup_frame(const CGLA::Vec3d &p) {
    int idx = this->get_field_index(p);
    return idx != -1 ? frame_list[idx]
                     : std::vector<CGLA::Vec3d>(dimF, CGLA::Vec3d(0.0));
}
// Lookup Layer at point.
std::vector<double> FrameField::lookup_layer(const CGLA::Vec3d &p) {
    int idx = this->get_field_index(p);
    return idx != -1 ? layer_list[idx] : std::vector<double>(dimF, 0.0);
}
// Lookup Density at point.
double FrameField::lookup_density(const CGLA::Vec3d &p) {
    int idx = this->get_field_index(p);
    return idx != -1 ? densi_list[idx] : 0.0;
}
// Lookup Position of node
CGLA::Vec3d FrameField::lookup_position(const CGLA::Vec3d &p) {
    int idx = this->get_field_index(p);
    return idx != -1 ? pos_list[idx] : CGLA::Vec3d(0.0);
}

// Function used to align the frames at the chosen indices
std::vector<std::vector<int>>
FrameField::align_index(const std::vector<size_t> &idx_list) {

    // Pre allocation
    std::vector<std::vector<int>> alignment(idx_list.size(),
                                            std::vector<int>(dimF, 0));
    std::vector<int>              aligned(idx_list.size());
    std::vector<double>           value(idx_list.size());

    for (int f_id = 0; f_id < dimF; f_id++) {

        // Setup variables for the reference frame
        alignment[0][f_id] = f_id;
        CGLA::Vec3d f0     = this->lookup_frame(idx_list[0])[f_id];

        for (size_t al = 1; al < idx_list.size(); al++) {
            value[al]                      = 0.0;
            std::vector<CGLA::Vec3d> frame = this->lookup_frame(idx_list[al]);
            for (int s = 0; s < dimF; s++) {

                CGLA::Vec3d f = frame[s];

                // Layer scaling
                double v = std::abs(CGLA::dot(f0, f));

                if (v <= value[al]) continue;

                value[al]           = v;
                alignment[al][f_id] = s;
            }
        }
    }

    return alignment;
}

// ========================================================================== //
// Single direction identification

// Find the index of the field direction closest to the specified vector d at
// point p.
int FrameField::nearest_index(const std::vector<CGLA::Vec3d> &field,
                              const CGLA::Vec3d &             d) {
    std::vector<double> alignment(field.size());
    for (size_t i = 0; i < field.size(); i++)
        alignment[i] = std::abs(CGLA::dot(field[i], d));

    return Utilities::max(alignment) > 0.0 ? Utilities::max_idx(alignment) : -1;
}

// Find the index of the field direction closest to the specified vector d at
// point p.
int FrameField::lookup_frame_index(const CGLA::Vec3d &p, const CGLA::Vec3d &d) {
    std::vector<CGLA::Vec3d> field = this->lookup_frame(p);
    return this->nearest_index(field, d);
}

// Find the field direction closest to the specified std::vector d at point p.
CGLA::Vec3d FrameField::lookup_frame_nearest(const CGLA::Vec3d &p,
                                             const CGLA::Vec3d &d) {
    std::vector<CGLA::Vec3d> field = this->lookup_frame(p);
    int                      idx   = this->nearest_index(field, d);
    return idx != -1 ? sgn(dot(d, field[idx])) * field[idx] : CGLA::Vec3d(0.0);
}
// Interpolate Layer thickness at point.
double FrameField::lookup_layer_nearest(const CGLA::Vec3d &p,
                                        const CGLA::Vec3d &d) {
    std::vector<double> layers = this->lookup_layer(p);
    int                 idx    = this->lookup_frame_index(p, d);
    return idx != -1 ? layers[idx] : 0.0;
}

// Find the index of the field direction closest to the specified vector d at
// point p.
int FrameField::interpolate_frame_index(const CGLA::Vec3d &p,
                                        const CGLA::Vec3d &d) {
    std::vector<CGLA::Vec3d> field = this->interpolate_frame(p);
    return this->nearest_index(field, d);
}
// Find the interpolated direction closest to the specified vector d at point p.
CGLA::Vec3d FrameField::interpolate_frame_nearest(const CGLA::Vec3d &p,
                                                  const CGLA::Vec3d &d) {
    std::vector<CGLA::Vec3d> field = this->interpolate_frame(p);
    int                      idx   = this->nearest_index(field, d);
    return idx != -1 ? sgn(dot(d, field[idx])) * field[idx] : CGLA::Vec3d(0.0);
}
// Interpolate Layer thickness at point.
double FrameField::interpolate_layer_nearest(const CGLA::Vec3d &p,
                                             const CGLA::Vec3d &d) {
    std::vector<double> layers = this->interpolate_layer(p);
    int                 idx    = this->lookup_frame_index(p, d);
    return idx != -1 ? layers[idx] : 0.0;
}

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