#include "config_loader.h"
#include <cstdio>
#include <cmath>
#include <cstring>
#include "simulation.h"
#include "maneuver.h"
#include "config_validator.h"
#include "orbital_mechanics.h"

static bool parse_toml_spacecraft(toml_datum_t craft_table, Spacecraft* craft);
static bool load_spacecraft_from_toml(SimulationState* sim, toml_result_t result);
static bool parse_toml_maneuver(toml_datum_t maneuver_table, Maneuver* maneuver, SimulationState* sim);
static bool load_maneuvers_from_toml(SimulationState* sim, toml_result_t result);

static bool extract_color_from_table(toml_datum_t table, float* color) {
    toml_datum_t r = toml_get(table, "r");
    toml_datum_t g = toml_get(table, "g");
    toml_datum_t b = toml_get(table, "b");

    // Accept both INT64 and FP64 for color components
    if ((r.type != TOML_FP64 && r.type != TOML_INT64) ||
        (g.type != TOML_FP64 && g.type != TOML_INT64) ||
        (b.type != TOML_FP64 && b.type != TOML_INT64)) {
        return false;
    }

    color[0] = (float)(r.type == TOML_FP64 ? r.u.fp64 : (double)r.u.int64);
    color[1] = (float)(g.type == TOML_FP64 ? g.u.fp64 : (double)g.u.int64);
    color[2] = (float)(b.type == TOML_FP64 ? b.u.fp64 : (double)b.u.int64);
    return true;
}

static bool parse_toml_orbit(toml_datum_t orbit_table, OrbitalElements* orbit, const char* object_name) {
    // Initialize orbital elements with defaults
    orbit->semi_major_axis = 0.0;
    orbit->eccentricity = 0.0;
    orbit->inclination = 0.0;
    orbit->longitude_of_ascending_node = 0.0;
    orbit->argument_of_periapsis = 0.0;
    orbit->true_anomaly = 0.0;

    // Parse semi_major_axis (for elliptical/hyperbolic) or semi_latus_rectum (for parabolic)
    toml_datum_t semi_major = toml_get(orbit_table, "semi_major_axis");
    toml_datum_t semi_latus = toml_get(orbit_table, "semi_latus_rectum");

    // Parse eccentricity first to determine which parameter is required
    toml_datum_t eccentricity = toml_get(orbit_table, "eccentricity");
    if (eccentricity.type != TOML_FP64) {
        printf("Error: Object '%s' missing required 'eccentricity' in orbit table\n", object_name);
        return false;
    }
    orbit->eccentricity = eccentricity.u.fp64;

    bool is_parabolic = (fabs(orbit->eccentricity - 1.0) < PARABOLIC_TOLERANCE);

    if (is_parabolic) {
        // Parabolic orbit - requires semi_latus_rectum
        if (semi_latus.type != TOML_FP64) {
            printf("Error: Parabolic orbit for object '%s' requires 'semi_latus_rectum' (not 'semi_major_axis')\n", object_name);
            return false;
        }
        orbit->semi_latus_rectum = semi_latus.u.fp64;
        if (semi_major.type == TOML_FP64) {
            printf("Warning: Object '%s' has parabolic eccentricity, 'semi_latus_rectum' used instead of 'semi_major_axis'\n", object_name);
        }
    } else {
        // Elliptical or hyperbolic - requires semi_major_axis
        if (semi_major.type == TOML_FP64) {
            orbit->semi_major_axis = semi_major.u.fp64;
        } else {
            printf("Error: Object '%s' must have 'semi_major_axis' in orbit table (non-parabolic orbits)\n", object_name);
            return false;
        }
        if (semi_latus.type == TOML_FP64) {
            printf("Warning: Object '%s' has non-parabolic eccentricity, 'semi_latus_rectum' ignored\n", object_name);
        }
    }

    // Parse true_anomaly (optional, default 0.0)
    toml_datum_t true_anomaly = toml_get(orbit_table, "true_anomaly");
    if (true_anomaly.type == TOML_FP64) {
        orbit->true_anomaly = true_anomaly.u.fp64;
    }

    // Parse inclination (optional, default 0.0)
    toml_datum_t inclination = toml_get(orbit_table, "inclination");
    if (inclination.type == TOML_FP64) {
        orbit->inclination = inclination.u.fp64;
    }

    // Parse longitude_of_ascending_node (optional, default 0.0)
    toml_datum_t raan = toml_get(orbit_table, "longitude_of_ascending_node");
    if (raan.type == TOML_FP64) {
        orbit->longitude_of_ascending_node = raan.u.fp64;
    }

    // Parse argument_of_periapsis (optional, default 0.0)
    toml_datum_t aop = toml_get(orbit_table, "argument_of_periapsis");
    if (aop.type == TOML_FP64) {
        orbit->argument_of_periapsis = aop.u.fp64;
    }

    return true;
}

static bool parse_toml_body(toml_datum_t body_table, CelestialBody* body) {
    // Extract string fields
    toml_datum_t name = toml_get(body_table, "name");
    if (name.type != TOML_STRING) {
        return false;
    }
    strncpy(body->name, name.u.s, 63);
    body->name[63] = '\0';

    // Extract numeric fields
    toml_datum_t mass = toml_get(body_table, "mass");
    toml_datum_t radius = toml_get(body_table, "radius");
    toml_datum_t parent_idx = toml_get(body_table, "parent_index");

    if (mass.type != TOML_FP64 || radius.type != TOML_FP64 ||
        parent_idx.type != TOML_INT64) {
        return false;
    }

    body->mass = mass.u.fp64;
    body->radius = radius.u.fp64;
    body->parent_index = (int)(parent_idx.type == TOML_INT64 ? parent_idx.u.int64 : (int)parent_idx.u.fp64);

    // Parse orbit table
    toml_datum_t orbit_table = toml_get(body_table, "orbit");
    if (orbit_table.type != TOML_TABLE) {
        printf("Error: Body '%s' missing required 'orbit' table\n", body->name);
        return false;
    }

    if (!parse_toml_orbit(orbit_table, &body->orbit, body->name)) {
        return false;
    }

    // Extract color
    toml_datum_t color = toml_get(body_table, "color");
    if (color.type != TOML_TABLE || !extract_color_from_table(color, body->color)) {
        return false;
    }

    // Initialize velocity and position (will be calculated later from orbital elements)
    body->global_position = {0.0, 0.0, 0.0};
    body->global_velocity = {0.0, 0.0, 0.0};
    body->local_position = {0.0, 0.0, 0.0};
    body->local_velocity = {0.0, 0.0, 0.0};
    body->soi_radius = 0.0;

    return true;
}

bool load_system_config(SimulationState* sim, const char* filepath) {
    toml_result_t result = toml_parse_file_ex(filepath);
    if (!result.ok) {
        printf("Error: Could not parse TOML config file: %s\n", filepath);
        printf("TOML Error: %s\n", result.errmsg);
        return false;
    }

    // Get bodies array
    toml_datum_t bodies = toml_get(result.toptab, "bodies");
    if (bodies.type != TOML_ARRAY) {
        printf("Error: Expected 'bodies' array in config file: %s\n", filepath);
        toml_free(result);
        return false;
    }

    // Count bodies first
    int body_count = bodies.u.arr.size;
    if (body_count == 0) {
        printf("Error: No bodies found in config file: %s\n", filepath);
        toml_free(result);
        return false;
    }

    if (body_count > sim->max_bodies) {
        printf("Error: Too many bodies (%d) for simulation (max: %d)\n",
               body_count, sim->max_bodies);
        toml_free(result);
        return false;
    }

    for (int i = 0; i < body_count; i++) {
        toml_datum_t body_table = bodies.u.arr.elem[i];
        if (!parse_toml_body(body_table, &sim->bodies[i])) {
            printf("Error: Failed to parse body at index %d\n", i);
            toml_free(result);
            return false;
        }
    }

    sim->body_count = body_count;

    strncpy(sim->config_name, filepath, sizeof(sim->config_name) - 1);
    sim->config_name[sizeof(sim->config_name) - 1] = '\0';

    // Load spacecraft from the same config file (before freeing TOML data)
    if (!load_spacecraft_from_toml(sim, result)) {
        toml_free(result);
        return false;
    }

    // Load maneuvers from the same config file (before freeing TOML data)
    if (!load_maneuvers_from_toml(sim, result)) {
        toml_free(result);
        return false;
    }

    toml_free(result);

    initialize_orbital_objects(sim);

    if (!run_all_config_validations(sim)) {
        printf("Error: Config validation failed\n");
        return false;
    }

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

static bool load_spacecraft_from_toml(SimulationState* sim, toml_result_t result) {
    toml_datum_t spacecraft = toml_get(result.toptab, "spacecraft");
    if (spacecraft.type != TOML_ARRAY) {
        return true;
    }

    int craft_count = spacecraft.u.arr.size;
    if (craft_count == 0) {
        return true;
    }

    if (craft_count > sim->max_craft) {
        printf("Error: Too many spacecraft (%d) for simulation (max: %d)\n",
               craft_count, sim->max_craft);
        return false;
    }

    for (int i = 0; i < craft_count; i++) {
        Spacecraft craft;
        toml_datum_t craft_table = spacecraft.u.arr.elem[i];
        if (!parse_toml_spacecraft(craft_table, &craft)) {
            printf("Error: Failed to parse spacecraft at index %d\n", i);
            return false;
        }

        int idx = add_spacecraft(sim, &craft);
        if (idx < 0) {
            printf("Error: Failed to add spacecraft to simulation\n");
            return false;
        }
    }

    printf("Loaded %d spacecraft from %s\n", craft_count, sim->config_name);
    return true;
}

static bool parse_toml_spacecraft(toml_datum_t craft_table, Spacecraft* craft) {
    toml_datum_t name = toml_get(craft_table, "name");
    if (name.type != TOML_STRING) {
        return false;
    }
    strncpy(craft->name, name.u.s, 63);
    craft->name[63] = '\0';

    toml_datum_t mass = toml_get(craft_table, "mass");
    toml_datum_t parent_idx = toml_get(craft_table, "parent_index");

    if (mass.type != TOML_FP64 || parent_idx.type != TOML_INT64) {
        return false;
    }

    craft->mass = mass.u.fp64;
    craft->parent_index = (int)(parent_idx.type == TOML_INT64 ? parent_idx.u.int64 : (int)parent_idx.u.fp64);

    // Parse orbit table
    toml_datum_t orbit_table = toml_get(craft_table, "orbit");
    if (orbit_table.type != TOML_TABLE) {
        printf("Error: Spacecraft '%s' missing required 'orbit' table\n", craft->name);
        return false;
    }

    if (!parse_toml_orbit(orbit_table, &craft->orbit, craft->name)) {
        return false;
    }

    // Initialize position and velocity (will be calculated later from orbital elements)
    craft->global_position = {0.0, 0.0, 0.0};
    craft->global_velocity = {0.0, 0.0, 0.0};
    craft->local_position = {0.0, 0.0, 0.0};
    craft->local_velocity = {0.0, 0.0, 0.0};

    return true;
}

static bool parse_toml_maneuver(toml_datum_t maneuver_table, Maneuver* maneuver, SimulationState* sim) {
    toml_datum_t name = toml_get(maneuver_table, "name");
    if (name.type != TOML_STRING) {
        return false;
    }
    strncpy(maneuver->name, name.u.s, 63);
    maneuver->name[63] = '\0';

    toml_datum_t spacecraft_name = toml_get(maneuver_table, "spacecraft_name");
    if (spacecraft_name.type != TOML_STRING) {
        return false;
    }

    int craft_idx = -1;
    for (int i = 0; i < sim->craft_count; i++) {
        if (strcmp(sim->spacecraft[i].name, spacecraft_name.u.s) == 0) {
            craft_idx = i;
            break;
        }
    }

    if (craft_idx < 0) {
        printf("Error: Maneuver '%s' references non-existent spacecraft '%s'\n",
               maneuver->name, spacecraft_name.u.s);
        return false;
    }

    maneuver->craft_index = craft_idx;

    toml_datum_t trigger_type_str = toml_get(maneuver_table, "trigger_type");
    if (trigger_type_str.type != TOML_STRING) {
        return false;
    }

    if (strcmp(trigger_type_str.u.s, "time") == 0) {
        maneuver->trigger_type = TRIGGER_TIME;
    } else if (strcmp(trigger_type_str.u.s, "true_anomaly") == 0) {
        maneuver->trigger_type = TRIGGER_TRUE_ANOMALY;
    } else {
        printf("Error: Unknown trigger_type '%s' for maneuver '%s'\n",
               trigger_type_str.u.s, maneuver->name);
        return false;
    }

    toml_datum_t trigger_value = toml_get(maneuver_table, "trigger_value");
    if (trigger_value.type != TOML_FP64 && trigger_value.type != TOML_INT64) {
        return false;
    }
    maneuver->trigger_value = trigger_value.type == TOML_FP64 ? trigger_value.u.fp64 : (double)trigger_value.u.int64;

    toml_datum_t direction_str = toml_get(maneuver_table, "direction");
    if (direction_str.type != TOML_STRING) {
        return false;
    }

    if (strcmp(direction_str.u.s, "prograde") == 0) {
        maneuver->direction = BURN_PROGRADE;
    } else if (strcmp(direction_str.u.s, "retrograde") == 0) {
        maneuver->direction = BURN_RETROGRADE;
    } else if (strcmp(direction_str.u.s, "normal") == 0) {
        maneuver->direction = BURN_NORMAL;
    } else if (strcmp(direction_str.u.s, "antinormal") == 0) {
        maneuver->direction = BURN_ANTINORMAL;
    } else if (strcmp(direction_str.u.s, "radial_in") == 0) {
        maneuver->direction = BURN_RADIAL_IN;
    } else if (strcmp(direction_str.u.s, "radial_out") == 0) {
        maneuver->direction = BURN_RADIAL_OUT;
    } else {
        printf("Error: Unknown direction '%s' for maneuver '%s'\n",
               direction_str.u.s, maneuver->name);
        return false;
    }

    toml_datum_t delta_v = toml_get(maneuver_table, "delta_v");
    if (delta_v.type != TOML_FP64 && delta_v.type != TOML_INT64) {
        return false;
    }
    maneuver->delta_v = delta_v.type == TOML_FP64 ? delta_v.u.fp64 : (double)delta_v.u.int64;

    maneuver->executed = false;
    maneuver->executed_time = 0.0;
    maneuver->scheduled_dt = 0.0;

    return true;
}

static bool load_maneuvers_from_toml(SimulationState* sim, toml_result_t result) {
    toml_datum_t maneuvers = toml_get(result.toptab, "maneuvers");
    if (maneuvers.type != TOML_ARRAY) {
        return true;
    }

    int maneuver_count = maneuvers.u.arr.size;
    if (maneuver_count == 0) {
        return true;
    }

    if (maneuver_count > sim->max_maneuvers) {
        printf("Error: Too many maneuvers (%d) for simulation (max: %d)\n",
               maneuver_count, sim->max_maneuvers);
        return false;
    }

    for (int i = 0; i < maneuver_count; i++) {
        Maneuver maneuver;
        toml_datum_t maneuver_table = maneuvers.u.arr.elem[i];
        if (!parse_toml_maneuver(maneuver_table, &maneuver, sim)) {
            printf("Error: Failed to parse maneuver at index %d\n", i);
            return false;
        }

        for (int j = 0; j < sim->maneuver_count; j++) {
            if (strcmp(sim->maneuvers[j].name, maneuver.name) == 0) {
                printf("Error: Duplicate maneuver name '%s' found at index %d\n",
                       maneuver.name, i);
                return false;
            }
        }

        sim->maneuvers[sim->maneuver_count] = maneuver;
        sim->maneuver_count++;
    }

    printf("Loaded %d maneuvers from %s\n", maneuver_count, sim->config_name);
    return true;
}