#include "FiniteElementMesh.h"

template<size_t P, size_t Q>
FiniteElementMesh<P, Q>::FiniteElementMesh(Mesh &m) {
    /* TODO: Changes to rotational motion implementation
        Initially, perform a straightforward conversion as currently implemented
        Define a new Boundary type called (called something like SlidingBoundary)
            SlidingBoundary holds a reference to the two boundaries and ensures the correct mapping
        In FiniteElementMesh, add a method called make_sliding(...) or something similar
            This method will implement a procedure which duplicates the nodes on the boundary and renumbers the nodes in the appropriate elements
        Then, the SlidingInterface boundary condition holds a pointer to the SlidingBoundary instead of two Boundary objects

        Perhaps a similar approach can be taken for implementing periodic boundary conditions (e.g. MappedBoundary holds two (or several arrays of) boundaries and enforces the proper invariant)
            e.g. make_mapped(...)
        Then PeriodicBoundaryCondition holds a pointer to the MappedBoundary instead of two Boundary objects
     */

    MeshData<P> md = m.mesh_data<P>();

    Nodes.reserve(md.Nodes.size());
    for (size_t i = 0; i != md.Nodes.size(); ++i) {
        Nodes.emplace_back(md.Nodes[i]);
    }

    for (auto b = ++md.Boundaries.begin(); b != md.Boundaries.end(); ++b) {
        Boundaries.emplace(b->first, std::make_shared<DiscreteBoundary>(b->first, b->second));
    }

    for (auto r = md.Regions.begin(); r != md.Regions.end(); ++r) {
        Regions.emplace(r->first, std::make_shared<DiscreteRegion>(r->first, r->second)); // TODO: Assign material properties here
    }

    Triangles = md.Triangles;
};

template<size_t P, size_t Q>
DiagonalMatrixGroup FiniteElementMesh<P, Q>::determinant() const {
    DiagonalMatrixGroup mat(TriangleQuadrature<Q>::size, Triangles.size());

    for (size_t i = 0; i != Triangles.size(); ++i) {
        Triangles[i].template determinant<Q>(mat, Nodes);
    }

    return mat;
}

template<size_t P, size_t Q>
DiagonalMatrixGroup FiniteElementMesh<P, Q>::weights() const {
    DiagonalMatrixGroup mat(TriangleQuadrature<Q>::size, Triangles.size());

    for (size_t i = 0; i != Triangles.size(); ++i) {
        Triangles[i].template determinant<Q>(mat, Nodes);
    }

    for (size_t q= 0; q != TriangleQuadrature<Q>::size; ++q) {
        mat(q) *= TriangleQuadrature<Q>::w[q];
    }

    return mat;
}

template<size_t P, size_t Q>
SparseMatrixGroup FiniteElementMesh<P, Q>::basis() const {
    SparseMatrixGroup mat(TriangleQuadrature<Q>::size, Nodes.size(), Triangles.size(), Triangle<P>::NumNodes);

    for (size_t i = 0; i != Triangles.size(); ++i) {
        Triangles[i].template basis<Q>(mat, Nodes);
    }

    return mat;
}

template<size_t P, size_t Q>
DerivativeMatrixGroup FiniteElementMesh<P, Q>::derivative() const {
    DerivativeMatrixGroup df(TriangleQuadrature<Q>::size, Nodes.size(), Triangles.size(), Triangle<P>::NumNodes);

    for (size_t i = 0; i != Triangles.size(); ++i) {
        Triangles[i].template derivative<Q>(df, Nodes);
    }

    return df;
}

template<size_t P, size_t Q>
SecondDerivativeMatrixGroup FiniteElementMesh<P, Q>::second_derivative() const {
    // TODO: Template based on derivative order template<size_t P, size_t Q, size_t O> DerivativeMatrixGroup<O> FiniteELementMesh<P,Q>::derivative<O>() const {...};
    SecondDerivativeMatrixGroup df(TriangleQuadrature<Q>::size, Nodes.size(), Triangles.size(), Triangle<P>::NumNodes);

    for (size_t i = 0; i != Triangles.size(); ++i) {
        Triangles[i].template second_derivative<Q>(df, Nodes);
    }

    return df;
}

template<size_t P, size_t Q>
std::vector<std::vector<XY>> FiniteElementMesh<P, Q>::quadrature_points() const {
    std::vector<std::vector<XY>> qp;
    qp.reserve(Triangles.size());

    for (size_t i = 0; i != Triangles.size(); ++i) {
        qp.push_back(Triangles[i].template quadrature_points<Q>(Nodes));
    }

    return qp;
}

template<size_t P, size_t Q>
std::vector<XY> FiniteElementMesh<P, Q>::quadrature_points(size_t i) const {
    std::vector<XY> qp = Triangles[i].template quadrature_points<Q>(Nodes);

    return qp;
}

// TODO: std::shared_ptr<DiscreteRegion> region(std::shared_ptr<Region const> const &r) const override {...}

template<size_t P, size_t Q>
std::vector<std::shared_ptr<DiscreteBoundary>> FiniteElementMesh<P, Q>::make_discontinuous(std::shared_ptr<Curve const> const &c) {
    // TODO: Should this be implemented in the refineable mesh instead? It might be simpler.
    std::vector<std::shared_ptr<DiscreteBoundary>> b_vec = boundary(c);

    if (b_vec.size() == 0 || b_vec.size() == 2) { // Boundary curve not found || boundary pair found
        return b_vec;
    } else if (b_vec.size() > 2) { // should not happen
        throw std::runtime_error("FiniteElementMesh::make_discontinuous expects to find at most 2 DiscreteBoundary objects");
    } // else b.size() == 1 and we need to construct an new boundary

    // Copy nodes
    std::vector<size_t> boundary_nodes = b_vec[0]->nodes();
    std::vector<size_t> discontinuous_nodes;
    discontinuous_nodes.reserve(boundary_nodes.size());
    for (size_t i = 0; i != boundary_nodes.size(); ++i) {
        discontinuous_nodes.push_back(Nodes.size());
        Nodes.emplace_back(Nodes[boundary_nodes[i]]);
    }

    // Construct new DiscontinuousBoundary
    bool is_closed = b_vec[0]->is_closed();
    std::shared_ptr<DiscreteBoundary> db = std::make_shared<DiscreteBoundary>(b_vec[0]->curve(), discontinuous_nodes, is_closed);
    Boundaries.emplace(c, db);

    //Boundaries.push_back(db);
    //sort_boundaries();

    // Renumber nodes in elements when the element is on the "right" side of the plane defined by the discontinuous boundary edge
    // (cross product of edge vector with edge midpoint to incenter vector is negative)

    // TODO: This a brute renumbering algorithm with O(t*n) complexity where t = Triangles.size() and n = bnodes.size()
    // TODO: Sort + renumber should result in O(n*log(t)) complexity, but all present assumptions about this class assume the the ordering of the elements never changes
    // TODO: However, triangles do store an ID which could reverse the sorting
    for (size_t i = 0; i != (discontinuous_nodes.size() - 1 + is_closed); ++i) {
        size_t ii = (i + 1) % discontinuous_nodes.size();
        double_t dx0 = Nodes[boundary_nodes[ii]].x() - Nodes[boundary_nodes[i]].x();
        double_t dy0 = Nodes[boundary_nodes[ii]].y() - Nodes[boundary_nodes[i]].y();

        for (Triangle<P> &t : Triangles) {
            for (size_t k = 0; k != 3 * P; ++k) { // TODO: This might not work for Element order > 2 having internal nodes, node numbering should occur in a spiral
                if (t.node(k) == boundary_nodes[i]) {
                    size_t k0 = t.node(0 * P);
                    size_t k1 = t.node(1 * P);
                    size_t k2 = t.node(2 * P);

                    double_t dx1 = (Nodes[k0].x() + Nodes[k1].x() + Nodes[k2].x()) / 3.0 - (Nodes[boundary_nodes[i]].x() + Nodes[boundary_nodes[ii]].x()) / 2.0;
                    double_t dy1 = (Nodes[k0].y() + Nodes[k1].y() + Nodes[k2].y()) / 3.0 - (Nodes[boundary_nodes[i]].y() + Nodes[boundary_nodes[ii]].y()) / 2.0;

                    double_t cross = dx0 * dy1 - dy0 * dx1;
                    if (cross < 0) {
                        t.node(k, discontinuous_nodes[i]);
                    }
                }

                if (t.node(k) == boundary_nodes[ii]) {
                    size_t k0 = t.node(0 * P);
                    size_t k1 = t.node(1 * P);
                    size_t k2 = t.node(2 * P);

                    double_t dx1 = (Nodes[k0].x() + Nodes[k1].x() + Nodes[k2].x()) / 3.0 - (Nodes[boundary_nodes[i]].x() + Nodes[boundary_nodes[ii]].x()) / 2.0;
                    double_t dy1 = (Nodes[k0].y() + Nodes[k1].y() + Nodes[k2].y()) / 3.0 - (Nodes[boundary_nodes[i]].y() + Nodes[boundary_nodes[ii]].y()) / 2.0;

                    double_t cross = dx0 * dy1 - dy0 * dx1;
                    if (cross < 0) {
                        t.node(k, discontinuous_nodes[ii]);
                    }
                }
            }
        }
    }

    // Renumber other boundary nodes when the other boundary is on the right-hand side of the plane defined by the discontinuous boundary edges
    b_vec.push_back(db);
    for (auto iter : Boundaries) {
        auto b = iter.second;

        if (b == b_vec[0] || b == b_vec[1]) {
            continue;
        }

        size_t k0, k1, k2;

        if (b->nodes().front() == boundary_nodes.front()) {
            k0 = b->node(0);
            k1 = b->node(1);
            k2 = boundary_nodes[1];

            double_t dx10 = Nodes[k1].x() - Nodes[k0].x();
            double_t dy10 = Nodes[k1].y() - Nodes[k0].y();

            double_t dx20 = Nodes[k2].x() - Nodes[k0].x();
            double_t dy20 = Nodes[k2].y() - Nodes[k0].y();

            double_t cross = dx10 * dy20 - dy10 * dx20;
            if (cross > 0) {
                b->node(0, discontinuous_nodes.front());
            }
        }

        if (b->nodes().front() == boundary_nodes.back()) {
            k0 = b->node(0);
            k1 = b->node(1);
            k2 = boundary_nodes[boundary_nodes.size() - 2];

            double_t dx10 = Nodes[k1].x() - Nodes[k0].x();
            double_t dy10 = Nodes[k1].y() - Nodes[k0].y();

            double_t dx20 = Nodes[k2].x() - Nodes[k0].x();
            double_t dy20 = Nodes[k2].y() - Nodes[k0].y();

            double_t cross = dx10 * dy20 - dy10 * dx20;
            if (cross < 0) {
                b->node(0, discontinuous_nodes.back());
            }
        }

        if (b->nodes().back() == boundary_nodes.front()) {
            k0 = b->node(b->nodes().size() - 1);
            k1 = b->node(b->nodes().size() - 2);
            k2 = boundary_nodes[1];

            double_t dx10 = Nodes[k1].x() - Nodes[k0].x();
            double_t dy10 = Nodes[k1].y() - Nodes[k0].y();

            double_t dx20 = Nodes[k2].x() - Nodes[k0].x();
            double_t dy20 = Nodes[k2].y() - Nodes[k0].y();

            double_t cross = dx10 * dy20 - dy10 * dx20;
            if (cross > 0) {
                b->node(b->nodes().size() - 1, discontinuous_nodes.front());
            }
        }

        if (b->nodes().back() == boundary_nodes.back()) {
            k0 = b->node(b->nodes().size() - 1);
            k1 = b->node(b->nodes().size() - 2);
            k2 = boundary_nodes[boundary_nodes.size() - 2];

            double_t dx10 = Nodes[k1].x() - Nodes[k0].x();
            double_t dy10 = Nodes[k1].y() - Nodes[k0].y();

            double_t dx20 = Nodes[k2].x() - Nodes[k0].x();
            double_t dy20 = Nodes[k2].y() - Nodes[k0].y();

            double_t cross = dx10 * dy20 - dy10 * dx20;
            if (cross < 0) {
                b->node(b->nodes().size() - 1, discontinuous_nodes.back());
            }
        }
    }

    return b_vec;
}

template<size_t P, size_t Q>
void FiniteElementMesh<P, Q>::write_scalar(Oe::VectorXd const &v, std::string path, std::string file_name) const {
    if (!std::experimental::filesystem::exists(path)) {
        std::experimental::filesystem::create_directories(path);
    }

    std::fstream fs;
    fs.open(path + file_name + ".oesc", std::fstream::out);

    // Write header
    size_t loc = 3;
    fs << loc << ',' << Triangles.size() * (P * P) << std::endl;
    loc += Triangles.size() * (P * P);

    fs << loc << ',' << Nodes.size() << std::endl;
    loc += Nodes.size();

    fs << loc << ',' << v.size() << std::endl;

    // Write triangles
    for (Triangle<P> const &t : Triangles) {
        fs << t;
        /*
        for (size_t i = 0; i != Triangle<P>::NumNodes - 1; ++i) {
            fs << t.node(i) << ',';
        }
        fs << t.node(Triangle<P>::NumNodes - 1) << std::endl;
         */
    }

    // Write nodes
    for (XY const &n : Nodes) {
        fs << n.x() << ',' << n.y() << std::endl;
    }

    // Write values
    for (size_t i = 0; i != v.size(); ++i) {
        fs << v(i) << std::endl;
    }

    fs.close();
}

template<size_t P, size_t Q>
void FiniteElementMesh<P, Q>::write_vector(Oe::ArrayXd const &Fx, Oe::ArrayXd const &Fy, std::string path, std::string file_name) const {
    if (!std::experimental::filesystem::exists(path)) {
        std::experimental::filesystem::create_directories(path);
    }

    std::fstream fs;
    fs.open(path + file_name + ".oeve", std::fstream::out);

    // Write header
    size_t loc = 3;
    fs << loc << ',' << Triangles.size() << std::endl;
    loc += Triangles.size();

    fs << loc << ',' << Nodes.size() << std::endl;
    loc += Nodes.size();

    fs << loc << ',' << Fx.size() << std::endl;

    // Write triangles
    for (Triangle<P> const &t : Triangles) {
        for (size_t i = 0; i != Triangle<P>::NumNodes - 1; ++i) {
            fs << t.node(i) << ',';
        }
        fs << t.node(Triangle<P>::NumNodes - 1) << std::endl;
    }

    // Write nodes
    for (XY const &n : Nodes) {
        fs << n.x() << ',' << n.y() << std::endl;
    }

    // Write values
    for (size_t i = 0; i != Fx.size(); ++i) {
        fs << Fx(i) << ',' << Fy(i) << std::endl;
    }

    fs.close();
}

// Explicit Instantiations
template class FiniteElementMesh<1, 1>;
template class FiniteElementMesh<1, 2>;

template class FiniteElementMesh<2, 1>;
template class FiniteElementMesh<2, 2>;
template class FiniteElementMesh<2, 4>;