#include "Rod2d.hpp"

#include "Eigen/Dense"

constexpr double M_Pl = 3.141592653589793238462643383279502884; /* pi */

void sqrt(Eigen::Matrix2d &mat) {
    const auto size = static_cast<size_t>(mat.size());
    for (size_t i = 0; i < size; i++) {
        mat.data()[i] = sqrt(mat.data()[i]);
    }
}
Rod2d::Rod2d(double L) : m_pos({1, 1}), m_e({1, 0}) {
    assert(L >= 1.0);
    if (L == 1.0) {
        m_D_rot = 3.0;
        m_Diff << 1, 0, 0, 1;
    } else {
        const double D0 = 3.0 * M_Pl / L;
        const double p = L;
        auto D_para =
            D0 / (M_Pl * 2.0) * (log(p) - 0.1401 + 1.034 / p - 0.228 / (p * p));
        auto D_ortho = D0 / (M_Pl * 4.0) *
                       (log(p) + 0.8369 + 0.5551 / p - 0.06066 / (p * p));
        m_D_rot = 3 * D0 / (M_Pl * L * L) *
                  (log(p) - 0.3512 + 0.7804 / p - 0.09801 / (p * p));
        m_Diff << D_para, 0, 0, D_ortho;
    }
    m_Diff_sqrt = m_Diff;
    m_D_rot_sqrt = sqrt(m_D_rot);
    sqrt(m_Diff_sqrt);
}

void Rod2d::setPos(const Eigen::Vector2d &Pos) { m_pos = Pos; }

double Rod2d::getDRot() const { return m_D_rot; }

const Eigen::Vector2d &Rod2d::getPos() const { return m_pos; }

const Eigen::Matrix2d &Rod2d::getDiff() const { return m_Diff; }

const Eigen::Matrix2d &Rod2d::getDiff_Sqrt() const { return m_Diff_sqrt; }

double Rod2d::getDRot_Sqrt() const { return m_D_rot_sqrt; }

const Eigen::Vector2d &Rod2d::getE() const { return m_e; }

void Rod2d::setE(const Eigen::Vector2d &mE) { m_e = mE; }
