# =============================================================================
# AI FIRST ENGINEERING LLC
# ENGINEERING COMPUTATIONAL PROJECT 1 — CAN WE PREDICT THE SHOT?
# Guided Python Computational Artifact — Version 3.2 (RELEASE LOCKED)
#
# Authors: Taner Onsay, Ph.D. & Sebnem Onsay
# © 2026 AI First Engineering LLC. All rights reserved.
# Educational use with attribution is permitted. Commercial reproduction,
# redistribution, adaptation, or resale requires written permission.
#
# PURPOSE
#   Use YOUR measured basketball data to:
#     1) read PARTIAL early-flight data from a CSV file,
#     2) verify that the release frame is t = 0,
#     3) fit x(t) and y(t),
#     4) extract release conditions,
#     5) predict the remaining trajectory using g = 9.81 m/s^2,
#     6) freeze/report the hoop-plane prediction,
#     7) AFTER the COMMIT Gate, read FULL validation data,
#     8) compare measured and predicted positions at matching times,
#     9) compute d_i and E_traj,
#    10) create engineering plots.
#
# AI may help explain or debug this program. You remain responsible for:
#   - using your own data files,
#   - understanding the equations,
#   - completing the STUDENT TASK lines,
#   - cross-checking results against Tracker and Excel,
#   - preserving your frozen Gate prediction.
#
# IMPORTANT:
#   This program NEVER silently changes the time origin.
#   If the first PARTIAL measurement is not at t ≈ 0, it stops.
#   In VALIDATION mode the prediction ALWAYS comes from the frozen COMMIT
#   state. PARTIAL is re-read only to plot it and to check that it has not
#   changed since the Gate. A saved committed state is never overwritten
#   unless you deliberately allow it BEFORE the Gate.
# =============================================================================

import numpy as np
import matplotlib.pyplot as plt
from pathlib import Path

# -----------------------------------------------------------------------------
# 0. STUDENT SETTINGS
# -----------------------------------------------------------------------------

SHOT_ID = "IMG_xxxx"                       # <-- STUDENT: assigned shot ID
EARLY_FILE = f"{SHOT_ID}_early.csv"         # columns: time,x,y

# Set this to None BEFORE the COMMIT Gate.
# AFTER the Gate and FULL-data release, enter your validation CSV filename.
VALIDATION_FILE = None                      # e.g. f"{SHOT_ID}_validation.csv"

# Created automatically in COMMIT mode. Keep it as a WORKING file; it is not
# a required submission. Validation uses it to preserve the frozen state.
COMMITTED_STATE_FILE = f"{SHOT_ID}_COMMITTED_STATE.csv"

# Leave False. Set True ONLY BEFORE the COMMIT Gate if you must re-run COMMIT
# mode to replace an earlier committed state (for example, after correcting
# your PARTIAL tracking). Never set True after FULL data are released.
REPLACE_COMMITTED_STATE = False

# ALTERNATE RECOVERY ROUTE: use ONLY if the committed-state file is missing
# after FULL release. Copy the frozen values from your COMMIT Gate / Workbook.
# Do NOT obtain these values by refitting FULL data. Leave as None normally.
FROZEN_FALLBACK = {
    "x_r": None, "h_r": None, "v_x0": None, "v_y0": None, "X_H": None
}

G = 9.81                                    # m/s^2 — ALWAYS use this in prediction
H = 3.048                                   # m — known physical rim-center height
X_H = None                                  # <-- STUDENT: hoop-center x coordinate from Tracker (m)

# The time exported from Tracker should begin at the first clean free-flight
# frame, t = 0.000 s. Small floating-point/export noise is allowed.
TIME_ZERO_TOL = 1.0e-4                      # s

# -----------------------------------------------------------------------------
# 1. SUPPLIED HELPER FUNCTIONS
# -----------------------------------------------------------------------------

def load_xyz_csv(filename):
    """Read a CSV with header and at least three columns: time, x, y."""
    path = Path(filename)

    if not path.exists():
        raise FileNotFoundError(
            f"\nCould not find: {filename}\n"
            "Check the filename and place the CSV in the same folder as this program."
        )

    data = np.loadtxt(path, delimiter=",", skiprows=1)

    if data.ndim == 1:
        data = data.reshape(1, -1)

    if data.shape[1] < 3:
        raise ValueError(
            f"{filename} must contain at least three columns: time, x, y."
        )

    t = data[:, 0]
    x = data[:, 1]
    y = data[:, 2]

    if len(t) < 3:
        raise ValueError(f"{filename} does not contain enough data points.")

    return t, x, y


def check_time_zero(t, filename):
    """
    Verify — do NOT repair — the Tracker time origin.
    """
    if abs(t[0]) > TIME_ZERO_TOL:
        raise ValueError(
            "\nTIME-ZERO CHECK FAILED\n"
            f"File: {filename}\n"
            f"First exported time = {t[0]:.6f} s, not approximately 0 s.\n\n"
            "Return to Tracker. Find the FIRST frame where the basketball is "
            "completely clear of the shooter's fingertips, set that release frame "
            "to t = 0.000 s, then export the data again.\n"
            "This program will NOT replace t with t - t[0]."
        )

    print(f"TIME-ZERO CHECK: PASS   first t = {t[0]:.6f} s")


def r_squared(measured, fitted):
    """Coefficient of determination for a fitted curve."""
    ss_res = np.sum((measured - fitted) ** 2)
    ss_tot = np.sum((measured - np.mean(measured)) ** 2)

    if np.isclose(ss_tot, 0.0):
        return np.nan

    return 1.0 - ss_res / ss_tot


def descending_hoop_crossing(x_r, h_r, v_x0, v_y0, x_h, H, g):
    """
    Solve y_p(t) = H and return the descending positive-time crossing.
    Also return x at that time and e_x = x_cross - x_h.
    """
    # h_r + v_y0*t - 0.5*g*t^2 = H
    roots = np.roots([-0.5 * g, v_y0, h_r - H])
    real_positive = [float(r.real) for r in roots
                     if abs(r.imag) < 1.0e-10 and r.real > 0.0]

    if not real_positive:
        raise ValueError(
            "The predicted trajectory does not produce a positive-time "
            "crossing of the hoop-height plane."
        )

    # The later positive root is the descending crossing.
    t_h = max(real_positive)
    x_cross = x_r + v_x0 * t_h
    e_x = x_cross - x_h
    return t_h, x_cross, e_x



def save_committed_state(filename, x_r, h_r, v_x0, v_y0, x_h):
    """Save the frozen release state used at the COMMIT Gate.
    Refuses to overwrite an existing committed state unless explicitly allowed."""
    if Path(filename).exists() and REPLACE_COMMITTED_STATE:
        evidence = [f for f in (f"{SHOT_ID}_validation.csv",
                                f"{SHOT_ID}_validation_trajectory.png")
                    if Path(f).exists()]
        if evidence:
            raise PermissionError(
                f"\nREPLACE_COMMITTED_STATE refused: FULL-data validation files exist "
                f"({', '.join(evidence)}).\n"
                "Replacement is allowed only BEFORE the COMMIT Gate and FULL-data release.\n"
                "Set REPLACE_COMMITTED_STATE = False. Your committed prediction stands."
            )
    if Path(filename).exists() and not REPLACE_COMMITTED_STATE:
        raise FileExistsError(
            f"\nA committed state already exists: {filename}\n"
            "It is your frozen COMMIT Gate prediction and will NOT be overwritten.\n"
            "- AFTER the Gate: set VALIDATION_FILE to your validation CSV instead.\n"
            "- BEFORE the Gate only: to replace it deliberately, set\n"
            "  REPLACE_COMMITTED_STATE = True for one run, then set it back to False."
        )
    arr = np.array([[x_r, h_r, v_x0, v_y0, x_h]], dtype=float)
    np.savetxt(
        filename, arr, delimiter=",",
        header="x_r,h_r,v_x0,v_y0,X_H", comments="", fmt="%.10g"
    )


def load_committed_state_or_fallback(filename, fallback):
    """Load the frozen state; if missing, require manual Gate/Workbook values."""
    path = Path(filename)
    if path.exists():
        data = np.genfromtxt(path, delimiter=",", names=True)
        print(f"FROZEN STATE: loaded {filename}")
        return tuple(float(data[name]) for name in ("x_r", "h_r", "v_x0", "v_y0", "X_H"))

    required = ("x_r", "h_r", "v_x0", "v_y0", "X_H")
    if all(fallback.get(k) is not None for k in required):
        print("FROZEN STATE FILE NOT FOUND — using MANUAL Gate/Workbook recovery values.")
        return tuple(float(fallback[k]) for k in required)

    raise FileNotFoundError(
        f"\nCould not find frozen state file: {filename}\n"
        "Do NOT refit after reveal to replace it. Enter the frozen COMMIT Gate / "
        "Project Workbook values in FROZEN_FALLBACK at the top of the program, "
        "then run validation again."
    )

# -----------------------------------------------------------------------------
# 2. READ PARTIAL EARLY-FLIGHT DATA
# -----------------------------------------------------------------------------

t, x, y = load_xyz_csv(EARLY_FILE)
check_time_zero(t, EARLY_FILE)

print("\n" + "=" * 72)
print("PARTIAL EARLY-FLIGHT DATA")
print("=" * 72)
print(f"Shot ID: {SHOT_ID}")
print(f"File:    {EARLY_FILE}")
print(f"N:       {len(t)} points")

# -----------------------------------------------------------------------------
# 3. FIT THE SAME DATA USED IN TRACKER AND EXCEL
# -----------------------------------------------------------------------------

# x(t) = A_x*t + B_x
A_x, B_x = np.polyfit(t, x, 1)

# y(t) = A_y*t^2 + B_y*t + C_y
A_y, B_y, C_y = np.polyfit(t, y, 2)

x_fit = A_x * t + B_x
y_fit = A_y * t**2 + B_y * t + C_y

R2_x = r_squared(x, x_fit)
R2_y = r_squared(y, y_fit)

# -----------------------------------------------------------------------------
# 4. RELEASE CONDITIONS — STUDENT ENGINEERING TASKS
# -----------------------------------------------------------------------------
# At t = 0:
#   x_r  = B_x
#   h_r  = C_y
#   v_x0 = A_x
#   v_y0 = B_y
#
# Complete the five lines marked STUDENT TASK.

# Replace each None with the appropriate engineering expression.
x_r  = None                              # STUDENT TASK 1
h_r  = None                              # STUDENT TASK 2
v_x0 = None                              # STUDENT TASK 3
v_y0 = None                              # STUDENT TASK 4
v_r  = None                              # STUDENT TASK 5 — release-speed magnitude

if any(value is None for value in (x_r, h_r, v_x0, v_y0, v_r)):
    raise NotImplementedError(
        "Complete STUDENT TASKS 1–5: derive the release state from the fitted "
        "coefficients and compute the release-speed magnitude."
    )

# Python trigonometric functions use radians internally.
theta_r = np.degrees(np.arctan2(v_y0, v_x0))

# Fit-health check ONLY. Do not use this as gravity in the prediction.
a_y_fit = 2.0 * A_y

print("\nFIT COEFFICIENTS")
print(f"A_x = {A_x:.6f} m/s")
print(f"B_x = {B_x:.6f} m")
print(f"A_y = {A_y:.6f} m/s^2")
print(f"B_y = {B_y:.6f} m/s")
print(f"C_y = {C_y:.6f} m")
print(f"R2_x = {R2_x:.6f}")
print(f"R2_y = {R2_y:.6f}")

print("\nRELEASE CONDITIONS" + ("" if VALIDATION_FILE is None
      else "  [current PARTIAL fit — integrity check only; NOT used for prediction]"))
print(f"x_r      = {x_r:.4f} m")
print(f"h_r      = {h_r:.4f} m")
print(f"v_x0     = {v_x0:.4f} m/s")
print(f"v_y0     = {v_y0:.4f} m/s")
print(f"v_r      = {v_r:.4f} m/s")
print(f"theta_r  = {theta_r:.3f} deg")
print(f"a_y,fit  = {a_y_fit:.4f} m/s^2   [health check; ideal is -9.81]")

# -----------------------------------------------------------------------------
# 5. PREDICT THE TRAJECTORY — STUDENT ENGINEERING TASKS
# -----------------------------------------------------------------------------
# The predictive model ALWAYS uses G = 9.81 m/s^2.
#
# Complete the two lines marked STUDENT TASK.

def predict_xy(t_query):
    t_query = np.asarray(t_query)

    x_pred = None                                  # STUDENT TASK 6
    y_pred = None                                  # STUDENT TASK 7

    if x_pred is None or y_pred is None:
        raise NotImplementedError(
            "Complete STUDENT TASKS 6–7: implement x_p(t) and y_p(t) using "
            "the frozen release state and G = 9.81 m/s^2."
        )

    return x_pred, y_pred


# Hoop-plane prediction used at the COMMIT Gate.
if X_H is None:
    raise ValueError(
        "Enter X_H at the top of the program using the hoop-center x coordinate "
        "measured in your established Tracker coordinate system."
    )

t_H_pred, x_H_cross_pred, e_x_pred = descending_hoop_crossing(
    x_r, h_r, v_x0, v_y0, X_H, H, G
)

# In COMMIT mode, write the state that will govern later validation.
# In VALIDATION mode, reload that frozen state (or require manual Gate recovery)
# so a later refit cannot silently change the prediction being tested.
if VALIDATION_FILE is None:
    save_committed_state(COMMITTED_STATE_FILE, x_r, h_r, v_x0, v_y0, X_H)
    print(f"FROZEN STATE SAVED: {COMMITTED_STATE_FILE}")
else:
    refit_state = (x_r, h_r, v_x0, v_y0, X_H)
    x_r, h_r, v_x0, v_y0, X_H = load_committed_state_or_fallback(
        COMMITTED_STATE_FILE, FROZEN_FALLBACK
    )
    frozen_state = (x_r, h_r, v_x0, v_y0, X_H)
    print("\nFROZEN RELEASE STATE (used for the prediction)")
    for name, val in zip(("x_r", "h_r", "v_x0", "v_y0", "X_H"), frozen_state):
        print(f"{name:8s} = {val:.4f}")
    if np.allclose(refit_state, frozen_state, rtol=1e-6, atol=1e-6):
        print("INTEGRITY CHECK: current PARTIAL fit and X_H match the frozen state.")
    else:
        print("INTEGRITY WARNING: current PARTIAL data or X_H differ from the frozen "
              "COMMIT state.\nThe FROZEN values are used. Explain the difference in your "
              "engineering assessment; do not change the committed prediction.")
    v_r = np.sqrt(v_x0**2 + v_y0**2)
    theta_r = np.degrees(np.arctan2(v_y0, v_x0))
    t_H_pred, x_H_cross_pred, e_x_pred = descending_hoop_crossing(
        x_r, h_r, v_x0, v_y0, X_H, H, G
    )
    print("VALIDATION WILL USE THE FROZEN COMMIT STATE ABOVE — not a new fit.")

print("\nFROZEN-PREDICTION INFORMATION")
print(f"x_H              = {X_H:.4f} m")
print(f"H                = {H:.4f} m")
print(f"t_H,pred         = {t_H_pred:.4f} s")
print(f"x_cross,pred     = {x_H_cross_pred:.4f} m")
print(f"e_x,pred         = {e_x_pred:.4f} m")
print("Prediction gravity = 9.81 m/s^2")

# Prediction curve long enough to include the descending hoop-height crossing.
t_plot_end = max(t_H_pred * 1.08, t[-1])
t_pred_plot = np.linspace(0.0, t_plot_end, 400)
x_pred_plot, y_pred_plot = predict_xy(t_pred_plot)

# -----------------------------------------------------------------------------
# 6. EARLY-FLIGHT FIT FIGURE
# -----------------------------------------------------------------------------

tt = np.linspace(t[0], t[-1], 250)

fig1, ax = plt.subplots(1, 2, figsize=(10, 4))

ax[0].plot(t, x, "o", label="measured x")
ax[0].plot(tt, A_x * tt + B_x, "-", label="linear fit")
ax[0].set_xlabel("t (s) [t = 0 at release]")
ax[0].set_ylabel("x (m)")
ax[0].set_title(f"x(t) linear fit, R² = {R2_x:.4f}")
ax[0].grid(True)
ax[0].legend()

ax[1].plot(t, y, "o", label="measured y")
ax[1].plot(tt, A_y * tt**2 + B_y * tt + C_y, "-", label="quadratic fit")
ax[1].set_xlabel("t (s) [t = 0 at release]")
ax[1].set_ylabel("y (m)")
ax[1].set_title(f"y(t) quadratic fit, R² = {R2_y:.4f}")
ax[1].grid(True)
ax[1].legend()

fig1.tight_layout()
if VALIDATION_FILE is None:   # do not overwrite Gate-time figures after the reveal
    fig1.savefig(f"{SHOT_ID}_fit_results.png", dpi=150)

# -----------------------------------------------------------------------------
# 7. COMMIT-PREDICTION FIGURE
# -----------------------------------------------------------------------------

fig2, ax2 = plt.subplots(figsize=(7, 5))
ax2.plot(x, y, "o", label="PARTIAL measured points")
ax2.plot(x_pred_plot, y_pred_plot, "-", label="FROZEN predicted trajectory")
ax2.plot(X_H, H, "s", label="hoop center")
ax2.set_xlabel("x (m)")
ax2.set_ylabel("y (m)")
ax2.set_title(f"{SHOT_ID} — committed trajectory prediction")
ax2.grid(True)
ax2.legend()
fig2.tight_layout()
if VALIDATION_FILE is None:   # do not overwrite Gate-time figures after the reveal
    fig2.savefig(f"{SHOT_ID}_committed_prediction.png", dpi=150)

# -----------------------------------------------------------------------------
# STOP HERE BEFORE THE COMMIT GATE
# -----------------------------------------------------------------------------

if VALIDATION_FILE is None:
    print("\n" + "=" * 72)
    print("COMMIT MODE")
    print("=" * 72)
    print("VALIDATION_FILE is None.")
    print("Use the results and committed-prediction figure for your Gate record.")
    print("Do NOT add FULL validation data until your instructor releases it.")
    plt.show()
    raise SystemExit

# -----------------------------------------------------------------------------
# 8. AFTER THE GATE — READ WITHHELD FULL VALIDATION DATA
# -----------------------------------------------------------------------------
# The validation CSV should contain ONLY the usable WITHHELD clean free-flight
# interval: first withheld frame after PARTIAL through the last clean free-flight
# frame before rim/net/backboard/other contact changes the physics.
#
# Keep ALL usable points. Do not thin the data.

t_val, x_meas, y_meas = load_xyz_csv(VALIDATION_FILE)

if np.any(t_val <= t[-1]):
    raise ValueError(
        "\nVALIDATION FILE CHECK FAILED\n"
        f"First validation time = {t_val[0]:.6f} s; last PARTIAL time = {t[-1]:.6f} s.\n"
        "The validation file must contain ONLY withheld frames AFTER the PARTIAL "
        "interval, on the SAME release-based clock (t = 0 at the release frame).\n"
        "Do not reset the clock at the first withheld frame, and do not include "
        "PARTIAL points."
    )

# Evaluate the FROZEN prediction at the SAME measured times.
x_pred_val, y_pred_val = predict_xy(t_val)

# -----------------------------------------------------------------------------
# 9. POINT-BY-POINT DISAGREEMENT — STUDENT ENGINEERING TASK
# -----------------------------------------------------------------------------
# d_i = sqrt[(x_meas - x_pred)^2 + (y_meas - y_pred)^2]
#
# Complete the two lines marked STUDENT TASK.

d_i = None                                                        # STUDENT TASK 8
E_traj = None                                                     # STUDENT TASK 9

if d_i is None or E_traj is None:
    raise NotImplementedError(
        "Complete STUDENT TASKS 8–9: compute pointwise spatial disagreement "
        "d_i and the RMS trajectory disagreement E_traj."
    )

# -----------------------------------------------------------------------------
# 10. MEASURED HOOP-PLANE CROSSING
# -----------------------------------------------------------------------------
# Estimate x where the measured FULL trajectory crosses y = H on the descending
# branch by linear interpolation between the two measured points that bracket H.

def measured_descending_hoop_crossing(t_m, x_m, y_m, H):
    candidates = []

    for i in range(len(t_m) - 1):
        # descending segment
        if y_m[i + 1] >= y_m[i]:
            continue

        y1, y2 = y_m[i], y_m[i + 1]

        # H lies between y1 and y2
        if (y1 - H) * (y2 - H) <= 0.0 and not np.isclose(y2, y1):
            fraction = (H - y1) / (y2 - y1)
            t_cross = t_m[i] + fraction * (t_m[i + 1] - t_m[i])
            x_cross = x_m[i] + fraction * (x_m[i + 1] - x_m[i])
            candidates.append((t_cross, x_cross))

    if not candidates:
        return np.nan, np.nan, np.nan

    # Use the latest descending crossing found.
    t_cross, x_cross = candidates[-1]
    e_x_meas = x_cross - X_H
    return t_cross, x_cross, e_x_meas


t_H_meas, x_H_cross_meas, e_x_meas = measured_descending_hoop_crossing(
    t_val, x_meas, y_meas, H
)

if np.isnan(e_x_meas):
    delta_e_x = np.nan
else:
    delta_e_x = e_x_meas - e_x_pred

print("\n" + "=" * 72)
print("PHYSICAL VALIDATION")
print("=" * 72)
print(f"Validation file: {VALIDATION_FILE}")
print(f"N_val = {len(t_val)} points")
print(f"E_traj = {E_traj:.5f} m = {100.0 * E_traj:.2f} cm")
print(f"e_x,pred = {e_x_pred:.5f} m")

if np.isnan(e_x_meas):
    print("e_x,meas = N/A — no clean measured descending hoop-height crossing before contact")
    print("Delta e_x = N/A — do not extrapolate through contact")
else:
    print(f"e_x,meas = {e_x_meas:.5f} m")
    print(f"Delta e_x = e_x,meas - e_x,pred = {delta_e_x:.5f} m")

# -----------------------------------------------------------------------------
# 11. VALIDATION FIGURE
# -----------------------------------------------------------------------------

# Extend the frozen prediction through the validation interval for plotting.
t_full_end = max(t_val[-1], t_H_pred * 1.08)
t_full_plot = np.linspace(0.0, t_full_end, 500)
x_full_pred, y_full_pred = predict_xy(t_full_plot)

fig3, ax3 = plt.subplots(figsize=(7, 5))
ax3.plot(x, y, "o", label="PARTIAL measured points")
ax3.plot(x_full_pred, y_full_pred, "-", label="FROZEN predicted trajectory")
ax3.plot(x_meas, y_meas, "o", label="withheld FULL measured points")
ax3.plot(X_H, H, "s", label="hoop center")
ax3.set_xlabel("x (m)")
ax3.set_ylabel("y (m)")
ax3.set_title(f"{SHOT_ID} — predicted vs measured trajectory")
ax3.grid(True)
ax3.legend()
fig3.tight_layout()
fig3.savefig(f"{SHOT_ID}_validation_trajectory.png", dpi=150)

# Optional disagreement-vs-time figure: useful for seeing whether the gap grows.
fig4, ax4 = plt.subplots(figsize=(7, 4))
ax4.plot(t_val, d_i, "o-")
ax4.set_xlabel("t (s)")
ax4.set_ylabel("spatial disagreement d_i (m)")
ax4.set_title(f"{SHOT_ID} — disagreement over withheld free flight")
ax4.grid(True)
fig4.tight_layout()
fig4.savefig(f"{SHOT_ID}_validation_disagreement.png", dpi=150)

print("\nENGINEERING REMINDERS")
print("- E_traj describes RMS spatial disagreement over the withheld free-flight interval.")
print("- Delta e_x compares measured and predicted hoop-plane crossing locations.")
print("- Neither metric is a make/miss classifier.")
print("- Do not invent uncertainty bounds from these results.")
print("- Preserve the frozen prediction and explain disagreement rather than repairing it.")

plt.show()
