#include "config_loader.h"
#include <cstdio>
#include <cstring>
#include <cmath>

// Parse a single body definition line
bool parse_body_line(const char* line, char* name, double* mass, double* radius,
                     Vec3* pos, int* parent_index, float* r, float* g, float* b,
                     double* eccentricity, double* semi_major_axis) {
    // Skip empty lines and comments
    if (line[0] == '\0' || line[0] == '#' || line[0] == '\n') {
        return false;
    }

    // Format: name mass radius x y z parent_index r g b eccentricity semi_major_axis
    int result = sscanf(line, "%63s %lf %lf %lf %lf %lf %d %f %f %f %lf %lf",
                       name, mass, radius, &pos->x, &pos->y, &pos->z,
                       parent_index, r, g, b, eccentricity, semi_major_axis);

    return result == 12;
}

struct OrbitParams {
    double eccentricity;
    double semi_major_axis;
};

// Forward declaration
void calculate_velocities(SimulationState* sim, OrbitParams* orbit_params);

// Load system configuration from file
bool load_system_config(SimulationState* sim, const char* filepath) {
    FILE* file = fopen(filepath, "r");
    if (!file) {
        printf("Error: Could not open config file: %s\n", filepath);
        return false;
    }

    char line[256];
    char name[64];
    double mass, radius;
    Vec3 pos;
    int parent_index;
    float r, g, b;
    double eccentricity, semi_major_axis;

    OrbitParams orbit_params[100];

    while (fgets(line, sizeof(line), file)) {
        if (parse_body_line(line, name, &mass, &radius, &pos, &parent_index, &r, &g, &b,
                           &eccentricity, &semi_major_axis)) {
            Vec3 vel = {0.0, 0.0, 0.0};
            add_body(sim, name, mass, radius, pos, vel, parent_index, r, g, b,
                    eccentricity, semi_major_axis);

            orbit_params[sim->body_count - 1].eccentricity = eccentricity;
            orbit_params[sim->body_count - 1].semi_major_axis = semi_major_axis;
        }
    }

    fclose(file);

    if (sim->body_count == 0) {
        printf("Error: No bodies loaded from config file\n");
        return false;
    }

    // Calculate initial velocities
    calculate_velocities(sim, orbit_params);

    // Calculate SOI radii
    calculate_soi_radii(sim);

    printf("Loaded %d bodies from %s\n", sim->body_count, filepath);
    return true;
}

// Calculate initial velocities using vis-viva equation
void calculate_velocities(SimulationState* sim, OrbitParams* orbit_params) {
    // First, handle multiple root bodies (binary stars, etc.)
    // Find all root bodies and calculate barycentric orbits
    int root_count = 0;
    int root_indices[32];  // Max 32 root bodies
    Vec3 barycenter = {0.0, 0.0, 0.0};
    double total_mass = 0.0;

    // Find all root bodies and calculate barycenter
    for (int i = 0; i < sim->body_count; i++) {
        if (sim->bodies[i].parent_index == -1) {
            if (root_count < 32) {
                root_indices[root_count++] = i;
                // Weighted sum for barycenter
                Vec3 weighted_pos = vec3_scale(sim->bodies[i].position, sim->bodies[i].mass);
                barycenter = vec3_add(barycenter, weighted_pos);
                total_mass += sim->bodies[i].mass;
            }
        }
    }

    // Calculate barycenter position
    if (total_mass > 0.0) {
        barycenter = vec3_scale(barycenter, 1.0 / total_mass);
    }

    // Debug output for multiple root bodies
    if (root_count > 1) {
        printf("\nBinary/Multiple star system detected:\n");
        printf("  Number of root bodies: %d\n", root_count);
        printf("  Barycenter position: (%.3e, %.3e, %.3e) m\n",
               barycenter.x, barycenter.y, barycenter.z);
        printf("  Total system mass: %.3e kg\n", total_mass);
    }

    // Set velocities for root bodies to orbit barycenter
    if (root_count > 1) {
        for (int i = 0; i < root_count; i++) {
            CelestialBody* body = &sim->bodies[root_indices[i]];

            // Calculate position relative to barycenter
            Vec3 r = vec3_sub(body->position, barycenter);
            double distance = vec3_magnitude(r);

            if (distance < 1.0) {
                body->velocity = {0.0, 0.0, 0.0};
                continue;
            }

            // Calculate total mass of OTHER root bodies
            double other_mass = total_mass - body->mass;

            // Calculate circular orbit speed around barycenter
            // v = sqrt(G * M_other / r)
            double speed = sqrt(G * other_mass / distance);

            // Create velocity perpendicular to position vector (same logic as below)
            Vec3 z_axis = {0.0, 0.0, 1.0};
            Vec3 vel_dir = {
                r.y * z_axis.z - r.z * z_axis.y,
                r.z * z_axis.x - r.x * z_axis.z,
                r.x * z_axis.y - r.y * z_axis.x
            };

            // If r is parallel to z-axis, use x-axis instead
            double cross_mag = vec3_magnitude(vel_dir);
            if (cross_mag < 0.01) {
                Vec3 x_axis = {1.0, 0.0, 0.0};
                vel_dir.x = r.y * x_axis.z - r.z * x_axis.y;
                vel_dir.y = r.z * x_axis.x - r.x * x_axis.z;
                vel_dir.z = r.x * x_axis.y - r.y * x_axis.x;
            }

            // Normalize and scale by orbital speed
            vel_dir = vec3_normalize(vel_dir);
            body->velocity = vec3_scale(vel_dir, speed);

            // Debug output
            printf("  %s: distance from barycenter = %.3e m, orbital speed = %.3e m/s\n",
                   body->name, distance, speed);
        }
    } else if (root_count == 1) {
        // Single root body stays stationary
        sim->bodies[root_indices[0]].velocity = {0.0, 0.0, 0.0};
    }

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

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

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

            // Calculate relative position
            Vec3 r = vec3_sub(body->position, parent->position);
            double distance = vec3_magnitude(r);

            if (distance < 1.0) {
                body->velocity = {0.0, 0.0, 0.0};
                continue;
            }

            double e = orbit_params[i].eccentricity;
            double a = orbit_params[i].semi_major_axis;

            // Use vis-viva equation: v = sqrt(G*M*(2/r - 1/a))
            double speed = sqrt(G * parent->mass * (2.0 / distance - 1.0 / a));

            if (e > 0.01) {
                printf("  %s: eccentric orbit (e=%.2f, a=%.3e m), speed at r=%.3e m: %.3f km/s\n",
                       body->name, e, a, distance, speed / 1000.0);
            }

            // Create velocity perpendicular to position vector
            // If position is mostly in XY plane, make velocity in XY plane
            // Cross product of r with z-axis gives perpendicular vector in XY plane
            Vec3 z_axis = {0.0, 0.0, 1.0};

            // Calculate cross product: r x z_axis
            Vec3 vel_dir = {
                r.y * z_axis.z - r.z * z_axis.y,
                r.z * z_axis.x - r.x * z_axis.z,
                r.x * z_axis.y - r.y * z_axis.x
            };

            // If r is parallel to z-axis, use x-axis instead
            double cross_mag = vec3_magnitude(vel_dir);
            if (cross_mag < 0.01) {
                Vec3 x_axis = {1.0, 0.0, 0.0};
                vel_dir.x = r.y * x_axis.z - r.z * x_axis.y;
                vel_dir.y = r.z * x_axis.x - r.x * x_axis.z;
                vel_dir.z = r.x * x_axis.y - r.y * x_axis.x;
            }

            // Normalize and scale by orbital speed
            vel_dir = vec3_normalize(vel_dir);
            body->velocity = vec3_scale(vel_dir, speed);

            // Add parent's velocity for absolute reference frame
            body->velocity = vec3_add(body->velocity, parent->velocity);
        }
    }
}

// Calculate SOI radii for all bodies
void calculate_soi_radii(SimulationState* sim) {
    for (int i = 0; i < sim->body_count; i++) {
        CelestialBody* body = &sim->bodies[i];

        if (body->parent_index == -1) {
            // Root body has very large SOI
            body->soi_radius = 1e15;  // ~1000 AU
        } else if (body->parent_index >= 0 && body->parent_index < sim->body_count) {
            CelestialBody* parent = &sim->bodies[body->parent_index];

            // Calculate semi-major axis (distance to parent)
            double semi_major_axis = vec3_distance(body->position, parent->position);

            // Update SOI using Hill sphere approximation
            update_soi(body, parent, semi_major_axis);
        }
    }
}