#include "ForceMulti.h"
ForceMulti::ForceMulti(size_t num_particles)
    : m_force(num_particles, {0.0, 0.0}), m_torque(num_particles, 0.0){};

Eigen::Vector2d ForceMulti::force(Eigen::Vector2d pos1, Eigen::Vector2d pos2,
                                  const Simulation& sim) {
    auto distance = sim.boundaryDistance(pos1, pos2);
    auto r = distance.norm();
    auto e = distance.normalized();
    return r * e;
}

double ForceMulti::torque(Eigen::Vector2d pos1, Eigen::Vector2d e1,
                          Eigen::Vector2d pos2, Eigen::Vector2d e2,
                          const Simulation& sim) {
    auto distance = sim.boundaryDistance(pos1, pos2);
    auto r = distance.norm();
    auto e = distance.normalized();
    return r;
}

void ForceMulti::force_torque(const Rod2dMulti& rods, const Simulation& sim) {
    for (size_t index_1 = 0; index_1 < rods.num_parts(); ++index_1) {
        for (size_t index_2 = index_1 + 1; index_2 < rods.num_parts();
             ++index_2) {
            auto f = force(rods.getPos(index_1), rods.getPos(index_2), sim);
            m_force[index_1] += f;
            m_force[index_2] -= f;

            auto t = torque(rods.getPos(index_1), rods.getE(index_1),
                            rods.getPos(index_2), rods.getE(index_2), sim);
            m_torque[index_1] += t;
            m_torque[index_2] -= t;
        }
    }
};

const std::vector<double>& ForceMulti::get_torque() const { return m_torque; }
const std::vector<Eigen::Vector2d>& ForceMulti::get_force() const {
    return m_force;
}
