/*
 * MIT License
 * Author: Mark Allyn
 *
 * simulator.cpp — simulated marine targets in Bellingham Bay.
 *
 * All targets are positioned relative to the fictitious marine
 * platform at 48.7436°N, 122.5647°W.
 *
 * Radar equation (marine X-band):
 *   P_r = FACTOR * sigma / R^4
 *   FACTOR = P_t * G^2 * lambda^2 / ((4*pi)^3 * L)
 *          ≈ 9890 W·m^4
 *
 * Brightness = clamp(SNR_dB / DYNAMIC_RANGE_DB, 0, 1)
 * SNR = P_r / MDS_WATTS
 */

#include "simulator.h"
#include <cmath>
#include <algorithm>

static constexpr float PI = 3.14159265f;

// Radar equation factor for marine X-band (see settings.h comments)
// FACTOR = P_t * G^2 * lambda^2 / ((4*pi)^3 * L)
// G = 4*pi*eta / (az_rad * el_rad) = 4*pi*0.6 / (0.00873 * 0.349) ≈ 2489
// FACTOR = 30000 * 2489^2 * 0.0325^2 / (1984.4 * 10) ≈ 9890
static constexpr float RADAR_FACTOR = 9890.0f;

float Simulator::computeBrightness(float sigmaM2, float rangeM) {
    if (rangeM < 1.0f) return 1.0f;
    float r4    = rangeM * rangeM * rangeM * rangeM;
    float P_r   = RADAR_FACTOR * sigmaM2 / r4;
    if (P_r <= MDS_WATTS) return 0.0f;
    float snr_dB = 10.0f * std::log10(P_r / MDS_WATTS);
    return std::clamp(snr_dB / RADAR_DYNAMIC_RANGE_DB, 0.0f, 1.0f);
}

Simulator::Simulator() {
    // ---- Initialise simulated vessels ----
    // Positions are in WGS84 lat/lon; headings and speeds drive movement.
    //
    // All positions in Bellingham Bay relative to platform at
    // 48.7436°N, 122.5647°W.

    int n = 0;

    // 1. Washington State ferry (southbound) — bearing 005°, 2.5 mi out
    targets_[n++] = { 1, 48.7797f, -122.5599f,  9.26f, 180.0f, 5000.0f, 80.0f };

    // 2. Bulk carrier near Bellingham terminal — bearing 090°, 1.8 mi
    targets_[n++] = { 2, 48.7436f, -122.5253f,  0.5f,   90.0f, 8000.0f, 75.0f };

    // 3. Fishing trawler NW — bearing 315°, 3.2 mi
    targets_[n++] = { 3, 48.7763f, -122.6143f,  3.08f,  40.0f,  200.0f, 12.0f };

    // 4. Recreational sailboat SW — bearing 225°, 2.4 mi
    targets_[n++] = { 4, 48.7191f, -122.6019f,  2.06f, 270.0f,    5.0f,  4.0f };

    // 5. Small powerboat SE — bearing 140°, 1.3 mi
    targets_[n++] = { 5, 48.7292f, -122.5464f,  6.17f,  95.0f,    3.0f,  3.0f };

    // 6. Stand-up paddleboarder NE — bearing 042°, 0.7 mi (marginal target)
    targets_[n++] = { 6, 48.7511f, -122.5544f,  0.26f,  90.0f, BOAT_SUP_RCS_M2, 1.0f };

    // 7. Large barge under tow N — bearing 355°, 4.8 mi
    targets_[n++] = { 7, 48.8127f, -122.5739f,  3.08f, 175.0f, 1000.0f, 30.0f };

    targetCount_ = n;
}

int Simulator::poll(float dt, std::array<TargetState, MAX_TARGETS>& out) {
    // Flat-earth conversion (valid for small distances around the platform)
    const float cosLat = std::cos(MARINE_PLATFORM_LAT * PI / 180.0f);

    for (int i = 0; i < targetCount_; ++i) {
        SimTarget& t = targets_[i];

        // Advance position
        float hdg_rad = t.headingDeg * PI / 180.0f;
        float dxM     = t.speedMps * dt * std::sin(hdg_rad);
        float dyM     = t.speedMps * dt * std::cos(hdg_rad);

        t.lonDeg += dxM / (METERS_PER_DEGREE * cosLat);
        t.latDeg += dyM /  METERS_PER_DEGREE;

        // Range and bearing from platform
        float dlatM = (t.latDeg - MARINE_PLATFORM_LAT)  * METERS_PER_DEGREE;
        float dlonM = (t.lonDeg - MARINE_PLATFORM_LON)  * METERS_PER_DEGREE * cosLat;

        float rangeM   = std::sqrt(dlonM * dlonM + dlatM * dlatM);
        float bearingR = std::atan2(dlonM, dlatM);  // CW from north
        float bearingD = bearingR * 180.0f / PI;
        if (bearingD < 0.0f) bearingD += 360.0f;

        out[i].id         = t.id;
        out[i].range_m    = rangeM;
        out[i].bearing_deg = bearingD;
        out[i].brightness = computeBrightness(t.sigmaM2, rangeM);
        out[i].size_m     = t.sizeM;
        out[i].valid      = true;
    }

    // Mark unused slots invalid
    for (int i = targetCount_; i < MAX_TARGETS; ++i)
        out[i].valid = false;

    return targetCount_;
}
