#include "test_utilities.h"
#include <cstdlib>
#include <cmath>

double calculate_kinetic_energy(CelestialBody* body) {
    double v_squared = body->velocity.x * body->velocity.x +
                       body->velocity.y * body->velocity.y +
                       body->velocity.z * body->velocity.z;
    return 0.5 * body->mass * v_squared;
}

double calculate_potential_energy_pair(CelestialBody* body1, CelestialBody* body2) {
    double distance = vec3_distance(body1->position, body2->position);
    if (distance < 1.0) distance = 1.0;
    return -G * body1->mass * body2->mass / distance;
}

double calculate_system_total_energy(SimulationState* sim) {
    double kinetic = 0.0;
    double potential = 0.0;

    for (int i = 0; i < sim->body_count; i++) {
        kinetic += calculate_kinetic_energy(&sim->bodies[i]);

        for (int j = i + 1; j < sim->body_count; j++) {
            potential += calculate_potential_energy_pair(&sim->bodies[i], &sim->bodies[j]);
        }
    }

    return kinetic + potential;
}

OrbitalMetrics calculate_orbital_metrics(CelestialBody* body, CelestialBody* parent) {
    OrbitalMetrics metrics;

    Vec3 relative_pos = vec3_sub(body->position, parent->position);
    metrics.orbital_radius = vec3_magnitude(relative_pos);
    metrics.velocity_magnitude = vec3_magnitude(body->velocity);
    metrics.angular_position = atan2(relative_pos.y, relative_pos.x);

    metrics.kinetic_energy = calculate_kinetic_energy(body);
    metrics.potential_energy = calculate_potential_energy_pair(body, parent);
    metrics.total_energy = metrics.kinetic_energy + metrics.potential_energy;

    return metrics;
}

OrbitTracker* create_orbit_tracker(int body_index) {
    OrbitTracker* tracker = (OrbitTracker*)malloc(sizeof(OrbitTracker));
    tracker->body_index = body_index;
    tracker->initial_angle = 0.0;
    tracker->previous_angle = 0.0;
    tracker->quadrant_transitions = 0;
    tracker->orbit_completed = false;
    tracker->time_at_completion = 0.0;
    tracker->min_time_days = 100.0;
    return tracker;
}

OrbitTracker* create_orbit_tracker_with_min_time(int body_index, double min_time_days) {
    OrbitTracker* tracker = (OrbitTracker*)malloc(sizeof(OrbitTracker));
    tracker->body_index = body_index;
    tracker->initial_angle = 0.0;
    tracker->previous_angle = 0.0;
    tracker->quadrant_transitions = 0;
    tracker->orbit_completed = false;
    tracker->time_at_completion = 0.0;
    tracker->min_time_days = min_time_days;
    return tracker;
}

void reset_orbit_tracker(OrbitTracker* tracker) {
    tracker->initial_angle = 0.0;
    tracker->previous_angle = 0.0;
    tracker->quadrant_transitions = 0;
    tracker->orbit_completed = false;
    tracker->time_at_completion = 0.0;
}

void update_orbit_tracker(OrbitTracker* tracker, CelestialBody* body, CelestialBody* parent, double current_time) {
    if (tracker->orbit_completed) return;

    Vec3 relative_pos = vec3_sub(body->position, parent->position);
    double current_angle = atan2(relative_pos.y, relative_pos.x);

    if (tracker->quadrant_transitions == 0) {
        tracker->initial_angle = current_angle;
        tracker->previous_angle = current_angle;
        tracker->quadrant_transitions = 1;
        return;
    }

    double angle_diff = current_angle - tracker->previous_angle;

    if (angle_diff > M_PI) {
        angle_diff -= 2.0 * M_PI;
        tracker->quadrant_transitions++;
    }
    if (angle_diff < -M_PI) {
        angle_diff += 2.0 * M_PI;
        tracker->quadrant_transitions++;
    }

    double total_rotation = current_angle - tracker->initial_angle;
    if (total_rotation < -M_PI) total_rotation += 2.0 * M_PI;
    if (total_rotation > M_PI) total_rotation -= 2.0 * M_PI;

    const double SECONDS_PER_DAY = 86400.0;
    double min_time_seconds = tracker->min_time_days * SECONDS_PER_DAY;

    if (tracker->quadrant_transitions >= 2 &&
        current_time > min_time_seconds &&
        fabs(total_rotation) < 0.05) {
        tracker->orbit_completed = true;
        tracker->time_at_completion = current_time;
    }

    tracker->previous_angle = current_angle;
}

void destroy_orbit_tracker(OrbitTracker* tracker) {
    free(tracker);
}

bool compare_double(double a, double b, double tolerance) {
    return fabs(a - b) <= tolerance;
}

bool compare_vec3(Vec3 a, Vec3 b, double tolerance) {
    return fabs(a.x - b.x) <= tolerance &&
           fabs(a.y - b.y) <= tolerance &&
           fabs(a.z - b.z) <= tolerance;
}