#include "body.hpp"
#include "calc.hpp"
#include <cmath>
#include <ranges>

Body::Body(const Position& X, const Velocity& V, const Mass& m, const std::string& name)
    : X(X), V(V), m(m), name(name) {
    A = Acceleration{Decimal(0), Decimal(0), Decimal(0)};
}

void Body::addAcceleration(const Acceleration& new_A) {
    for (const auto& [new_a, cur_a]: std::views::zip(new_A, this->A)){
        cur_a += new_a;
    }
}

void Body::subAcceleration(const Acceleration& new_A) {
    for (const auto& [new_a, cur_a]: std::views::zip(new_A, this->A)){
        cur_a -= new_a;
    }
}

std::tuple<Position, Velocity, Mass> Body::save() const {
    return std::make_tuple(X, V, m);
}

Body Body::load(const std::tuple<Position, Velocity, Mass>& tup) {
    return Body(std::get<0>(tup), std::get<1>(tup), std::get<2>(tup));
}

void Body::step(Decimal step_size) {
    for (int i = 0; i < 3; ++i) {
        X[i] += step_size * V[i];
        V[i] += step_size * A[i];
        A[i] = Decimal(0);
    }
}

Decimal Body::E() const {
    return ke() + pe();
}

Decimal Body::pe() const {
    return -m / dist_from_o();
}

Decimal Body::dist_from_o() const {
    Decimal sum = Decimal(0);
    for (const auto& x : X) {
        sum += x * x;
    }
    return std::sqrt(sum);
}

Decimal Body::ke() const {
    return Decimal(0.5) * m * (_speed() * _speed());
}

Decimal Body::_speed() const {
    Decimal sum = Decimal(0);
    for (const auto& v : V) {
        sum += v * v;
    }
    return std::sqrt(sum);
}

std::string Body::speed() const {
    return format_sig_figs(real_vel(_speed()), 5);
}

std::string Body::toString() const {
    std::string pos_str, vel_str;
    for (int i = 0; i < 3; ++i) {
        pos_str += format_sig_figs(real_pos(X[i]), 10) + "m ";
        vel_str += format_sig_figs(real_vel(V[i]), 10) + "m/s ";
    }
    return name + ": X = " + pos_str + ", V = " + vel_str;
}