#include "MMPLib.h"
#include "src/geodesic_algorithm_exact.h"
#include <iostream>
#include <iomanip>
#include <fstream>
#include <map>
#include <cmath>
#include "GmshGlobal.h"
#include "GModel.h"
#include "MTriangle.h"

// forward declarations of helper functions
geodesic::SurfacePoint projectIntoFace(double *coords, int *status,geodesic::Mesh* meshMMP);
void projectPoint(double* coord, double* coordP, geodesic::face_pointer face);
void localCoordinateTriangle(double* coords, geodesic::face_pointer face, double* u, double* v, double* w);
geodesic::base_pointer findElementFallInside(double* coords, double* coordP, int* status, geodesic::vertex_pointer closestVertex);
geodesic::Vertex* findClosestVertex(double* coords, int* status,geodesic::Mesh* meshMMP);
double getDistance_(geodesic::SurfacePoint &pt, geodesic::GeodesicAlgorithmExact *algoMMP);

MMPLib::~MMPLib() 
{ 
    delete static_cast<geodesic::GeodesicAlgorithmExact*>(_algoMMP);
    delete static_cast<geodesic::Mesh*>(_meshMMP);
}

void MMPLib::setPointSource(int idxVertex) { sources.push_back(idxVertex);}
void MMPLib::setStopPoint(int idxVertex) {stops.push_back(idxVertex);}

void MMPLib::propagate() 
{ 
  geodesic::GeodesicAlgorithmExact* algoMMP=static_cast<geodesic::GeodesicAlgorithmExact*>(_algoMMP);
  std::vector<geodesic::SurfacePoint> sources_; // source points
  std::vector<geodesic::SurfacePoint> stops_; // source points
  for (auto it=sources.begin();it!=sources.end();++it)
  {
      sources_.push_back(geodesic::SurfacePoint(&static_cast<geodesic::Mesh*>(_meshMMP)->vertices()[*it]));
  }
  for (auto it=stops.begin();it!=stops.end();++it)
  {
      stops_.push_back(geodesic::SurfacePoint(&static_cast<geodesic::Mesh*>(_meshMMP)->vertices()[*it]));
  }
  if(stops.empty()) algoMMP->propagate(sources_); 
  else algoMMP->propagate(sources_, geodesic::GEODESIC_INF, &stops_);   
}


void MMPLib::propagate(double start[3]) 
{ 
  int status;
  std::vector<geodesic::SurfacePoint> sources_; // source points
  std::vector<geodesic::SurfacePoint> stops_; // source points
  for (auto it=stops.begin();it!=stops.end();++it)
  {
      stops_.push_back(geodesic::SurfacePoint(&static_cast<geodesic::Mesh*>(_meshMMP)->vertices()[*it]));
  }
  geodesic::GeodesicAlgorithmExact* algoMMP=static_cast<geodesic::GeodesicAlgorithmExact*>(_algoMMP);
  geodesic::Mesh* meshMMP = static_cast<geodesic::Mesh*>(_meshMMP);
  geodesic::SurfacePoint ptStart = projectIntoFace(start, &status,meshMMP);
  sources_.push_back(ptStart);
  if(stops.empty()) algoMMP->propagate(sources_); 
  else algoMMP->propagate(sources_, geodesic::GEODESIC_INF, &stops_);   
}

MMPLib::MMPLib(std::string mshFile){

    // std::cout << "Reading mesh file " << mshFile << std::endl;

    // 1. gmsh file
    // read and get nodes and elements
    GmshInitialize();

    GModel model = GModel();
    model.readMSH(mshFile);
    
    // 2. build mesh data for MMP

    std::size_t numTriangle = model.getNumMeshElements(2); // number of triangles

    // std::cout << "  " << numTriangle << " triangles found" << std::endl;

    // read msh from triangle to triangle

    GModel::fiter itFace = model.firstFace();
    GModel::fiter itLastFace = model.lastFace();

    std::map<std::size_t,std::vector<size_t>> elementsRead;
    std::map<std::size_t,std::vector<double>> nodeRead;


    for(; itFace != itLastFace; itFace++){
        GFace* faceIt = *itFace;
        for(MTriangle* tri : faceIt->triangles){
            std::vector<size_t> triVertex(3);
            for(int i=0;i<3;i++){
                triVertex[i] = tri->getVertex(i)->getNum();
                nodeRead.emplace(triVertex[i],std::vector<double>{tri->getVertex(i)->x(),tri->getVertex(i)->y(),tri->getVertex(i)->z()});
            }

            elementsRead.insert({tri->getNum(),triVertex});
        }
    }

    int numPoint = nodeRead.size();
    int numTri = elementsRead.size();

    // std::cout << "  num element : " << numTri << " - num node : " << numPoint << std::endl;

    // int idxHelp = 0;
    // auto itHelp = elementsRead.begin();
    // while(idxHelp < 5){
    //     std::cout << "  " << itHelp->first;
    //     for(int j=0; j<3; j++) std::cout << " " << itHelp->second[j];
    //     std::cout << std::endl;
    //     itHelp++;
    //     idxHelp++;
    // }

    // idxHelp = 0;
    // auto itHelp2 = nodeRead.begin();
    // while(idxHelp < 5){
    //     std::cout << "  " << itHelp2->first;
    //     for(int j=0; j<3; j++) std::cout << " " << itHelp2->second[j];
    //     std::cout << std::endl;
    //     itHelp2++;
    //     idxHelp++;
    // }

    // 3. create MMP algo object

    std::vector<double> nodeCoord(3*numPoint);
    std::map<std::size_t,std::size_t> idxMap;
    int shiftIdx = 0;

    for(const auto& [idx,coords] : nodeRead){
        for(int j=0; j<3; ++j) nodeCoord[shiftIdx + j] = coords[j];
        idxMap.insert({idx,shiftIdx/3});
        shiftIdx += 3;
    }

    // std::cout << "Map idx :" << std::endl;
    // auto itHelp3 = idxMap.begin();
    // for(int i=0; i < 5; i++){
    //     std::cout << " " << itHelp3->first << " " << itHelp3->second << std::endl;
    //     itHelp3++;
    // }

    std::vector<std::size_t> eleList(3*numTri);
    shiftIdx = 0;

    for(const auto& [idx,pts] : elementsRead){
        for(int j=0; j<3; ++j) eleList[shiftIdx + j] = idxMap[pts[j]];
        shiftIdx += 3;
    }

    // std::cout << "  table of nodes have : " << nodeCoord.size()/3 << " - table of elements : " << eleList.size()/3 << std::endl;
    // int nbrShow = 10;
    // std::cout << "Here are the first nodes :" << std::endl;
    // for(int i=0; i < 3*nbrShow; i+=3) std::cout << " " << i/3 << "  " << nodeCoord[i] << " " << nodeCoord[i+1] << " " << nodeCoord[i+2] << std::endl;
    // std::cout << "Here are the first elements :" << std::endl;
    // for(int i=0; i < 3*nbrShow; i+=3) std::cout << " " << i/3 << "  " << eleList[i] << " " << eleList[i+1] << " " << eleList[i+2] << std::endl;

    geodesic::Mesh* meshMMP = new geodesic::Mesh;
    _meshMMP = meshMMP;
    meshMMP->initialize_mesh_data(numPoint,nodeCoord,numTri,eleList);
    // meshMMP.initialize_mesh_data(nodeCoord,eleList);
    geodesic::GeodesicAlgorithmExact *algoMMP = new geodesic::GeodesicAlgorithmExact(meshMMP);
    _algoMMP = algoMMP;
    
    // 5. close
    //GmshFinalize();
}

double MMPLib::getDistance(int idxVertex)
{
    geodesic::GeodesicAlgorithmExact *algoMMP = static_cast<geodesic::GeodesicAlgorithmExact *>(_algoMMP);
    geodesic::Mesh* meshMMP = static_cast<geodesic::Mesh*>(_meshMMP);
    geodesic::SurfacePoint target(&meshMMP->vertices()[idxVertex]);
    double distance;
    unsigned best_source = algoMMP->best_source(target,distance);
    return distance;
}

double MMPLib::getDistance(double target[3])
{
   int status;
   geodesic::vertex_pointer vxEnd = findClosestVertex(target,&status,static_cast<geodesic::Mesh*>(_meshMMP));
   double distance=getDistance(vxEnd->id());
   return distance;
}

int MMPLib::printMSHDistances(std::string mshFile)
{
    // MSH 2 format
    geodesic::Mesh* meshMMP = static_cast<geodesic::Mesh*>(_meshMMP);
    std::ofstream createdFile(mshFile);

    if(!createdFile){
        std::cerr << "Error : cannot print msh file";
        return 0;
    }

    createdFile << std::fixed << std::setprecision(10);

    createdFile << "$MeshFormat\n2.2 0 8\n$EndMeshFormat\n$Nodes\n";
    std::vector<geodesic::Vertex> nodes = meshMMP->vertices();

    createdFile << nodes.size() << "\n";

    for(geodesic::Vertex& nd : nodes) createdFile << nd.id()+1 << " " << nd.x() << " " << nd.y() << " " << nd.z() << "\n";

    // note : meshMMP start node index at 0 but MSH at 1

    createdFile << "$EndNodes\n$Elements\n";

    std::vector<geodesic::Face> triangles = meshMMP->faces();

    createdFile << triangles.size() << "\n";

    for(geodesic::Face& tri : triangles){
        createdFile << tri.id()+1 << " 2 2 0 0 ";
        createdFile << tri.adjacent_vertices()[0]->id()+1 << " " << tri.adjacent_vertices()[1]->id()+1 << " " << tri.adjacent_vertices()[2]->id()+1 << "\n";
    }

    createdFile << "$EndElements\n$NodeData\n1\n\"Distance MMP\"\n1\n0.0\n3\n0\n1\n" << nodes.size() << "\n";

    for(geodesic::Vertex& nd : nodes){
        createdFile << nd.id()+1 << " " << this->getDistance(nd.id()) << "\n";
    }

    createdFile << "$EndNodeData";

    createdFile.close();

    return 1;
}

void MMPLib::getVertex(int idxVertex, std::vector<double> &coords)
{
    geodesic::Mesh* meshMMP = static_cast<geodesic::Mesh*>(_meshMMP);
    coords.clear();
    geodesic::Vertex coordVertex = meshMMP->vertices()[idxVertex];
    coords = {coordVertex.x(), coordVertex.y(), coordVertex.z()};
}

int MMPLib::getNumberVertices() { return static_cast<geodesic::Mesh*>(_meshMMP)->vertices().size(); }


// out of class helper functions


geodesic::Vertex *findClosestVertex(double *coords, int *status,geodesic::Mesh* meshMMP)
{
    double distanceClosest = geodesic::GEODESIC_INF;
    double distTemp;
    geodesic::Vertex* closestVertex = nullptr;
    for(geodesic::Vertex& tryVertex : meshMMP->vertices()){
        distTemp = sqrt(pow(tryVertex.x()-coords[0],2) + pow(tryVertex.y()-coords[1],2) + pow(tryVertex.z()-coords[2],2));
        if(distTemp < distanceClosest){
            closestVertex = &tryVertex;
            distanceClosest = distTemp;
        }
    }

    if(status) *status = (distanceClosest < 1e-9) ? 1 : 0;

    return closestVertex;
}

geodesic::base_pointer findElementFallInside(double *coords, double *coordP, int *status, geodesic::vertex_pointer closestVertex)
{
    geodesic::base_pointer element = nullptr;
    geodesic::face_pointer landFace = nullptr;
    geodesic::MeshElementBase::face_pointer_vector adjFaces = closestVertex->adjacent_faces();
    double u, v, w;

    // std::cout << "findElementFallInside\n closest Vertex : (" << closestVertex->id() << ")  " << closestVertex->x() << " " << closestVertex->y() << " " << closestVertex->z()  << std::endl;
    // std::cout << "  for this vertex, there are " << adjFaces.size() << " triangles." << std::endl;

    for(geodesic::face_pointer tryFace : adjFaces){
        // std::cout << " test face " << tryFace->id() << std::endl;
        // std::cout << "   with nodes (" << tryFace->adjacent_vertices()[0]->id() << ") " << tryFace->adjacent_vertices()[0]->x() << " " << tryFace->adjacent_vertices()[0]->y() << " " << tryFace->adjacent_vertices()[0]->z() << std::endl;
        // std::cout << "              (" << tryFace->adjacent_vertices()[1]->id() << ") " << tryFace->adjacent_vertices()[1]->x() << " " << tryFace->adjacent_vertices()[1]->y() << " " << tryFace->adjacent_vertices()[1]->z() << std::endl;
        // std::cout << "              (" << tryFace->adjacent_vertices()[2]->id() << ") " << tryFace->adjacent_vertices()[2]->x() << " " << tryFace->adjacent_vertices()[2]->y() << " " << tryFace->adjacent_vertices()[2]->z() << std::endl;

        // compute projected point
        projectPoint(coords, coordP, tryFace);

        // std::cout << "   projection : " << coordP[0] << " " << coordP[1] << " " << coordP[2] << std::endl;

        // compute intern coord
        localCoordinateTriangle(coordP, tryFace, &u, &v, &w);

        // std::cout << "  coord local : " << u << " " << v << " " << w << std::endl;

        // get out if inside
        if(u < -1e-9 || v < -1e-9 || w < -1e-9){
            continue;
        } else {
            // std::cout << "  face found" << std::endl;
            landFace = tryFace;
            break;
        }
    }

    if(!landFace){
        *status = 3;
        return nullptr;
    }

    *status = 2;

    if(u < 1e-9){ // on edge 01
        for(geodesic::edge_pointer ed : landFace->adjacent_edges()){
            if(ed->belongs(landFace->adjacent_vertices()[0]) && ed->belongs(landFace->adjacent_vertices()[1])){
                element = ed;
                break;
            }
        }
    }
    else if(v < 1e-9){ // on edge 02
        for(geodesic::edge_pointer ed : landFace->adjacent_edges()){
            if(ed->belongs(landFace->adjacent_vertices()[0]) && ed->belongs(landFace->adjacent_vertices()[2])){
                element = ed;
                break;
            }
        }
    }
    else if(w < 1e-9){ // on edge 12
        for(geodesic::edge_pointer ed : landFace->adjacent_edges()){
            if(ed->belongs(landFace->adjacent_vertices()[1]) && ed->belongs(landFace->adjacent_vertices()[2])){
                element = ed;
                break;
            }
        }
    }
    else {
        *status = 0;
        element = landFace;
    }

    // if(element->type() == geodesic::EDGE){
    //     std::cout << " point on edge " << element->adjacent_vertices()[0]->id() << " - " << element->adjacent_vertices()[1]->id() << std::endl;
    // }
    // if(element->type() == geodesic::FACE){
    //     std::cout << " point on face " << element->id() << std::endl;
    // }

    return element;
}

geodesic::SurfacePoint projectIntoFace(double *coords, int *status,geodesic::Mesh* meshMMP)
{
    int stat;
    *status = 0;

    geodesic::vertex_pointer closestVertex = findClosestVertex(coords, &stat,meshMMP);

    if(stat != 0){ // the coordinates correspond to an existing vertex
        *status = 1;
        return geodesic::SurfacePoint(closestVertex);
    }

    double coordP[3];

    geodesic::base_pointer element = findElementFallInside(coords, coordP, &stat, closestVertex);

    if(stat == 2){ // the projection falls on a edge
        *status = 2;
        return geodesic::SurfacePoint(element,coordP[0],coordP[1],coordP[2],geodesic::EDGE);
    }

    // the projection falls on a surface
    return geodesic::SurfacePoint(element,coordP[0],coordP[1],coordP[2],geodesic::FACE);
}

void projectPoint(double *coord, double *coordP, geodesic::face_pointer face)
{
    geodesic::MeshElementBase::vertex_pointer_vector vs = face->adjacent_vertices();
    double vec1[3] = { vs[1]->x() - vs[0]->x(), vs[1]->y() - vs[0]->y(), vs[1]->z() - vs[0]->z() };
    double vec2[3] = { vs[2]->x() - vs[0]->x(), vs[2]->y() - vs[0]->y(), vs[2]->z() - vs[0]->z() };

    double normal[3] = { vec1[1] * vec2[2] - vec1[2] * vec2[1], vec1[2] * vec2[0] - vec1[0] * vec2[2], vec1[0] * vec2[1] - vec1[1] * vec2[0] };
    double normalLength = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]);

    double vP[3] = { coord[0] - vs[0]->x(), coord[1] - vs[0]->y(), coord[2] - vs[0]->z() };

    double normalDist = 0.;
    for(int i = 0; i < 3; i++) normalDist += vP[i]*normal[i];

    for(int i = 0; i < 3; i++) coordP[i] = coord[i] - normal[i]*normalDist/normalLength;

}

void localCoordinateTriangle(double *coords, geodesic::face_pointer face, double *u, double *v, double *w)
{
    geodesic::MeshElementBase::vertex_pointer_vector vs = face->adjacent_vertices();
    double vec1[3] = { vs[1]->x() - vs[0]->x(), vs[1]->y() - vs[0]->y(), vs[1]->z() - vs[0]->z() };
    double vec2[3] = { vs[2]->x() - vs[0]->x(), vs[2]->y() - vs[0]->y(), vs[2]->z() - vs[0]->z() };
    double vP[3] = { coords[0] - vs[0]->x(), coords[1] - vs[0]->y(), coords[2] - vs[0]->z() };

    double d00 = 0., d01 = 0., d11 = 0., d20 = 0., d21 = 0.;
    for(int i = 0; i < 3; i++){
        d00 += vec1[i]*vec1[i];
        d01 += vec1[i]*vec2[i];
        d11 += vec2[i]*vec2[i];
        d20 += vP[i]*vec1[i];
        d21 += vP[i]*vec2[i];
    }

    double denom = d00 * d11 - d01 * d01;

    *v = (d11 * d20 - d01 * d21) / denom;
    *u = (d00 * d21 - d01 * d20) / denom;
    *w = 1. - *v - *u;
}


double getDistance(geodesic::SurfacePoint &pt, geodesic::GeodesicAlgorithmExact *algoMMP)
{
    double distance;
    unsigned best_source = algoMMP->best_source(pt,distance);

    return distance;
}