#ifndef DER_LEIMER_HPP
#define DER_LEIMER_HPP

//#include "ElasticRod.hh"
#include "simulationmodel.hpp"
#include <Eigen/Sparse>

class DER_leimer : public SimulationModel
{
public:
    DER_leimer();
    virtual SimulationResults executeSimulation(const std::shared_ptr<SimulationJob> &pJob) override;
    struct RodParams
    {
        double thickness;
        double kstretching; //stretching stiffness
        double kbending;    //bending stiffness
        double ktwist;      //twisting stiffness
        double kanchorpos;  //anchor positional stiffness
        double kanchordir;  //anchor directional stiffness
        double rho;         //density
        double restlength;
    };

    struct RodState
    {
        Eigen::MatrixXd centerlinePositions;
        Eigen::VectorXi centerlineIndices; //"global" indices of centerline vertices
        Eigen::MatrixXd
            directors; //normal vectors to each segment. They are called "directors" here because thats how they call them in the weaving geodesics paper
        Eigen::MatrixXd matDirNormal;   //Normal vector of each segment
        Eigen::MatrixXd matDirBinormal; //Binormal vector of each segment
        Eigen::VectorXd thetas;
        Eigen::MatrixXd centerlineVelocities;
        Eigen::VectorXd directorAngVel;
        Eigen::MatrixXd curvatures;
    };

    class Rod
    {
    public:
        Rod(const RodState &startState,
            const Eigen::VectorXd &widths,
            RodParams &params,
            bool isClosed);

        enum RodVisibilityState { RS_TRANSLUCENT = 0, RS_VISIBLE, RS_HIDDEN };

        bool isClosed() const { return isClosed_; }

        void setVisibilityState(RodVisibilityState state) { visState_ = state; }
        RodVisibilityState visibilityState() const { return visState_; }

        Eigen::Vector3d rodColor() const;
        Eigen::Vector3d rodColor(int seg) const;
        int rodColorID() const { return colorID_; }
        void cycleColor();
        int numVertices() const { return (int) curState.centerlinePositions.rows(); }
        int numSegments() const { return (int) curState.thetas.size(); }
        double arclength() const;
        RodState curState;
        RodState startState;

        Eigen::VectorXd widths;
        Eigen::VectorXd restlens;
        Eigen::VectorXd masses;
        Eigen::VectorXd momInertia;
        Eigen::VectorXi segColors;
        Eigen::MatrixXd vSegParams; // stretch, bend, twist, restlength
        RodParams params;

        void initializeRestQuantities();

    private:
        int colorID_;
        bool isClosed_;
        RodVisibilityState visState_;
    };

    struct Anchor
    {
        int rod;
        int seg;
        double bary; // where along the segment should the anchor be placed?[0,1]. 0 at the beginning , 1 at the end
        Eigen::Vector3d position;
        Eigen::Vector3d normal;
        double ratio; //??
        double k_pos;
        double k_dir;
    };

    struct Intersection
    {
        int intersectionIndex;
        int globalVi;
        Eigen::MatrixXi rodSegmentIndices;
        Eigen::Matrix3d rotationMatrix;
    };

    struct SimParams
    {
        bool allowSliding;
        Eigen::MatrixXd *anchorPoints;
        Eigen::MatrixXd *anchorNormals;
        bool gravityEnabled{true};
        Eigen::Vector3d gravityDir;
        double floorHeight;
        double floorWeight;
    };

    class RodConfig
    {
    public:
        ~RodConfig();

        void addRod(Rod *rod);
        void addAnchor(Anchor a);
        void addIntersection(Intersection inter);
        void initWeave(); // Call after all constraints initialized
        int numRods() const { return (int) rods.size(); }
        int numAnchors() const { return (int) anchors.size(); }
        int numVertices() const { return (int) vertices.rows(); }
        int numIntersections() const { return (int) intersections.size(); }
        void reset();
        void createVisualizationMesh(Eigen::MatrixXd &Q, Eigen::MatrixXi &F);
        Eigen::Vector3d shadeRodSegment(Eigen::Vector3d light,
                                        int rod,
                                        int segment,
                                        bool showCover) const;
        void saveRodGeometry(const std::string &prefix);

        std::vector<Rod *> rods;
        std::vector<Anchor> anchors;
        std::vector<Intersection> intersections;
        Eigen::MatrixXd vertices;

        bool showConstraints;
        bool allowSliding;
        double decay; //?
        int getNumDoF() const;
    };

    void setStructure(const std::shared_ptr<SimulationMesh> &pMesh) override;

private:
    bool simulateOneStepGrid(RodConfig *config);

    void rAndJGrid(const RodConfig &config,
                   Eigen::VectorXd &r,
                   Eigen::SparseMatrix<double> *Jr,
                   double &gravityPE,
                   Eigen::VectorXd &gravityForces,
                   const SimParams &params,
                   double &externalForcesPE,
                   const Eigen::VectorXd &externalForces = Eigen::VectorXd(),
                   std::vector<Eigen::Triplet<int>> *H_index = NULL,
                   std::vector<double> *H_value = NULL,
                   Eigen::SparseMatrix<double> *Ju = NULL,
                   std::vector<Eigen::Triplet<int>> *Hu_index = NULL,
                   std::vector<double> *Hu_value = NULL);

    RodConfig *readRodGrid(const std::shared_ptr<SimulationMesh> &pMesh,
                           const std::vector<int> &numVertsPerRod,
                           const std::vector<int> &vInd,
                           const std::vector<Eigen::Vector3d> &verts,
                           const std::vector<Eigen::Vector3d> &binorms,
                           const std::vector<RodParams> &rodparams,
                           const std::vector<Anchor> &anchors,
                           const double decay = 1.0,
                           double *vStiffness = 0,
                           double *restVerts = 0,
                           double *restNormals = 0,
                           const std::vector<double> &thetas = std::vector<double>(),
                           double *eulers = 0);
    double lineSearchGrid(RodConfig &config,
                          const Eigen::VectorXd &update,
                          const SimParams &params,
                          const Eigen::VectorXd &externalForces = Eigen::VectorXd());
    SimulationResults constructSimulationResults(const std::shared_ptr<SimulationJob> &pJob,
                                                 RodConfig *config) const;
    Eigen::Vector3d getPerpendicularVector(const Eigen::Vector3d &t) const;

    double energy;
    Eigen::VectorXd updateVec;
    double forceResidual;
    int iter{0};
    std::shared_ptr<SimulationJob> pJob;

    //    std::unique_ptr<ElasticRod> pRod;
};

Eigen::Matrix3d eulerRotation(double e0, double e1, double e2, int deriv = -1, int deriv2 = -1);
#endif // DER_LEIMER_HPP