#include "bodies.h"
#include <cstdlib>
#include <cstring>
#include <cmath>

// Create a new simulation
SimulationState* create_simulation(int max_bodies, double time_step) {
    SimulationState* sim = (SimulationState*)malloc(sizeof(SimulationState));
    sim->bodies = (CelestialBody*)malloc(sizeof(CelestialBody) * max_bodies);
    sim->body_count = 0;
    sim->max_bodies = max_bodies;
    sim->time = 0.0;
    sim->dt = time_step;
    return sim;
}

// Destroy simulation and free memory
void destroy_simulation(SimulationState* sim) {
    if (sim) {
        if (sim->bodies) {
            free(sim->bodies);
        }
        free(sim);
    }
}

// Add a celestial body to the simulation
void add_body(SimulationState* sim, const char* name, double mass, double radius,
              Vec3 pos, Vec3 vel, int parent_index, float r, float g, float b,
              double eccentricity, double semi_major_axis) {
    if (sim->body_count >= sim->max_bodies) {
        return;  // No more space
    }

    CelestialBody* body = &sim->bodies[sim->body_count];
    strncpy(body->name, name, 63);
    body->name[63] = '\0';
    body->mass = mass;
    body->radius = radius;
    body->position = pos;
    body->velocity = vel;
    body->soi_radius = 0.0;  // Will be calculated later
    body->parent_index = parent_index;
    body->color[0] = r;
    body->color[1] = g;
    body->color[2] = b;
    body->eccentricity = eccentricity;
    body->semi_major_axis = semi_major_axis;

    sim->body_count++;
}

// Find which body is gravitationally dominant for the given body
int find_dominant_body(SimulationState* sim, int body_index) {
    if (body_index < 0 || body_index >= sim->body_count) {
        return -1;
    }

    CelestialBody* body = &sim->bodies[body_index];
    int dominant = body->parent_index;

    // Check all other bodies to see if we're within their SOI
    for (int i = 0; i < sim->body_count; i++) {
        if (i == body_index) continue;

        CelestialBody* potential_parent = &sim->bodies[i];
        double distance = vec3_distance(body->position, potential_parent->position);

        // If we're within this body's SOI and it's not our current parent
        if (distance < potential_parent->soi_radius && i != dominant) {
            // Check if this body is more dominant (closer or more massive)
            if (dominant == -1) {
                dominant = i;
            } else {
                CelestialBody* current_parent = &sim->bodies[dominant];
                double dist_to_current = vec3_distance(body->position, current_parent->position);

                // Switch if this potential parent is significantly closer
                if (distance < dist_to_current * 0.5) {
                    dominant = i;
                }
            }
        }
    }

    return dominant;
}

// Update sphere of influence radius using Hill sphere approximation
// r_soi = a * (m/M)^(2/5) where a = semi-major axis, m = body mass, M = parent mass
void update_soi(CelestialBody* body, CelestialBody* parent, double semi_major_axis) {
    if (parent == NULL || parent->mass <= 0.0) {
        // Root body (like Sun) has infinite SOI, use a large value
        body->soi_radius = 1e15;  // 1000 AU in meters
        return;
    }

    double mass_ratio = body->mass / parent->mass;
    body->soi_radius = semi_major_axis * pow(mass_ratio, 0.4);  // 2/5 = 0.4
}

// Update the entire simulation by one time step
void update_simulation(SimulationState* sim) {
    // First, update root bodies (they interact with each other)
    for (int i = 0; i < sim->body_count; i++) {
        CelestialBody* body = &sim->bodies[i];

        if (body->parent_index == -1) {
            // This is a root body - calculate forces from OTHER root bodies
            Vec3 total_force = {0.0, 0.0, 0.0};

            for (int j = 0; j < sim->body_count; j++) {
                if (i == j) continue;  // Don't apply force to itself

                CelestialBody* other = &sim->bodies[j];
                if (other->parent_index == -1) {
                    // Other is also a root body - apply gravitational force
                    Vec3 force = calculate_gravity_force(body, other);
                    total_force = vec3_add(total_force, force);
                }
            }

            // Apply total force from all other root bodies
            Vec3 acceleration = calculate_acceleration(total_force, body->mass);
            euler_step(body, acceleration, sim->dt);
        }
    }

    // Now update non-root bodies (planets, moons, etc.)
    for (int i = 0; i < sim->body_count; i++) {
        CelestialBody* body = &sim->bodies[i];

        // Skip root bodies (already updated above)
        if (body->parent_index == -1) {
            continue;
        }

        // Check if parent has changed (SOI transition)
        int new_parent = find_dominant_body(sim, i);
        if (new_parent != body->parent_index && new_parent != -1) {
            body->parent_index = new_parent;
        }

        // Get the current parent
        if (body->parent_index >= 0 && body->parent_index < sim->body_count) {
            CelestialBody* parent = &sim->bodies[body->parent_index];

            // Calculate gravitational force from parent
            Vec3 force = calculate_gravity_force(body, parent);

            // Calculate acceleration
            Vec3 acceleration = calculate_acceleration(force, body->mass);

            // Perform Euler integration step
            euler_step(body, acceleration, sim->dt);
        }
    }

    // Update simulation time
    sim->time += sim->dt;
}