#include "Rod2dMulti.hpp"
#include "Eigen/Dense"
#include "tools.hpp"

Rod2dMulti::Rod2dMulti(double length, size_t num_particles)
    : m_pos(num_particles, Eigen::Vector2d({1, 1})),
      m_e(num_particles, {1, 0}) {
    assert(L >= 1.0);
    if (length == 1.0) {
        m_D_rot = 3;
        m_Diff << 1, 0, 0, 1;
    } else {
        const double D0 = 3.0 * Tools::M_Pl / length;
        const double p = length;
        auto D_para = Tools::D_para(D0, p);
        auto D_ortho = Tools::D_ortho(D0, p);
        m_D_rot = Tools::D_rot(D0, p);
        m_Diff << D_para, 0, 0, D_ortho;
    }
    m_Diff_sqrt = m_Diff;
    m_D_rot_sqrt = sqrt(m_D_rot);
    Tools::mat_sqrt(m_Diff_sqrt);
}

void Rod2dMulti::setPos(const Eigen::Vector2d &Pos, size_t pos) {
    assert(pos < m_pos.size());
    m_pos[pos] = Pos;
}

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

const Eigen::Vector2d &Rod2dMulti::getPos(size_t pos) const {
    return m_pos[pos];
}

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

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

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

const Eigen::Vector2d &Rod2dMulti::getE(size_t pos) const { return m_e[pos]; }
size_t Rod2dMulti::num_parts() const{
return m_e.size();
}
void Rod2dMulti::setE(const Eigen::Vector2d &mE, size_t pos) { m_e[pos] = mE; }
