(*---------------------------------------------------------------------------
   Copyright (c) 2025 Thomas Gazagnaire. All rights reserved.
   SPDX-License-Identifier: ISC
  ---------------------------------------------------------------------------*)

(** P0 flight simulator — minimal init process for the flight partition.

    1. Mount devtmpfs, proc, sysfs 2. Open /dev/virtio-ports/ipc for writing 3.
    Propagate ISS orbit via SGP4 and stream continuous telemetry *)

let banner =
  {|
 ____   ___
|  _ \ / _ \
| |_) | | | |
|  __/| |_| |
|_|    \___/  Flight Simulator
|}

let pr fmt = Fmt.pr (fmt ^^ "@.")
let ipc_name = "ipc"

let write_frame port (frame : Space_wire.Msg.t) =
  let buf = Bytes.make Space_wire.Msg.frame_size '\x00' in
  Wire.Codec.encode Space_wire.Msg.codec frame buf 0;
  Zephyr.Virtio_port.write_all port (Bytes.unsafe_to_string buf)

let send port kind ~apid payload =
  pr "Sending %a: %s" Space_wire.Msg.pp_kind kind payload;
  let frame = Space_wire.Msg.v kind ~apid payload in
  write_frame port frame

(* ISS TLE (epoch 2024) — good enough for a continuous demo. *)
let iss_tle =
  {|ISS (ZARYA)
1 25544U 98067A   24001.00000000  .00016717  00000-0  10270-3 0  9003
2 25544  51.6400 100.0000 0007417  50.0000 310.1280 15.49560000000017|}

let rand_float lo hi = lo +. Random.float (hi -. lo)
let rand_pick a = a.(Random.int (Array.length a))

(* Simulate slowly-varying onboard sensors *)
let temp = ref 22.0
let voltage = ref 3.3
let cpu = ref 15

let drift_sensor () =
  temp := !temp +. rand_float (-0.3) 0.3;
  temp := Float.max 10.0 (Float.min 40.0 !temp);
  voltage := !voltage +. rand_float (-0.02) 0.02;
  voltage := Float.max 3.0 (Float.min 3.6 !voltage);
  cpu := !cpu + (Random.int 5 - 2);
  cpu := max 5 (min 95 !cpu)

let send_housekeeping port seq =
  drift_sensor ();
  match seq mod 5 with
  | 0 -> send port TM ~apid:0x100 "heartbeat:ok"
  | 1 ->
      send port TM ~apid:0x102
        (Fmt.str "sensor:temp=%.1f,voltage=%.2f" !temp !voltage)
  | 2 ->
      send port HEALTH ~apid:0x100
        (Fmt.str "cpu=%d%%,mem=%dMB,uptime=%d" !cpu
           (48 + Random.int 20)
           (seq * 6))
  | 3 ->
      let evt =
        rand_pick
          [|
            "ORBIT_UPDATE:epoch";
            "CMD_ACK:0x42";
            "BEACON_SENT";
            "WATCHDOG_PET";
            "SENSOR_CAL:temp:done";
          |]
      in
      send port EVR ~apid:0x200 evt
  | _ ->
      let mode = rand_pick [| "nominal"; "nominal"; "nominal"; "safe" |] in
      send port TM ~apid:0x103 (Fmt.str "status:%s" mode)

let run_sim port =
  let tle =
    match Sgp4.parse_tle_string iss_tle with
    | Ok t -> t
    | Error e -> Fmt.failwith "TLE parse: %a" Sgp4.pp_error e
  in
  let state =
    match Sgp4.init tle with
    | Ok s -> s
    | Error e -> Fmt.failwith "SGP4 init: %a" Sgp4.pp_error e
  in
  let gsto = Sgp4.gsto state in
  pr "SGP4 initialized: %s (epoch=%.1f)" tle.name (Sgp4.epoch_unix tle);
  (* Initial boot burst *)
  send port TM ~apid:0x100 "heartbeat:ok";
  Unix.sleepf 0.2;
  send port EVR ~apid:0x200 "IPC_LINK_UP:p1";
  Unix.sleepf 0.2;
  send port EVR ~apid:0x200 "SGP4_INIT:ok";
  Unix.sleepf 0.2;
  (* Continuous telemetry loop — 1 Hz orbit + mixed housekeeping *)
  pr "Entering continuous telemetry loop...";
  let seq = ref 0 in
  while true do
    let tsince = float_of_int !seq *. 0.1 in
    (* 0.1 min = 6s per tick *)
    incr seq;
    (* Orbit propagation *)
    (match Sgp4.propagate state tle tsince with
    | Ok (pos, vel) ->
        let speed =
          sqrt ((vel.vx *. vel.vx) +. (vel.vy *. vel.vy) +. (vel.vz *. vel.vz))
        in
        let alt =
          sqrt ((pos.x *. pos.x) +. (pos.y *. pos.y) +. (pos.z *. pos.z))
          -. 6378.135
        in
        let lat, lon, _alt_geo = Sgp4.to_geodetic pos gsto tsince in
        send port TM ~apid:0x101
          (Fmt.str "orbit:lat=%.2f,lon=%.2f,alt=%.1f,v=%.2f" lat lon alt speed)
    | Error _ -> send port ERROR ~apid:0x1FF "sgp4:propagation_error");
    Unix.sleepf 0.3;
    send_housekeeping port !seq;
    Unix.sleepf 0.7
  done

let () =
  Random.self_init ();
  Eio_main.run @@ fun env ->
  Eio.Switch.run @@ fun _sw ->
  let fs = Eio.Stdenv.fs env in
  let pid = Unix.getpid () in
  Fmt.pr "%s@." banner;
  pr "P0 Flight Simulator (PID %d)" pid;
  if pid = 1 then begin
    pr "Setting up filesystems...";
    Pid1.setup_filesystems ~fs;
    Pid1.setup_signals ();
    pr "Waiting for IPC device...";
    if not (Zephyr.Virtio_port.wait ipc_name ~timeout:5.0) then begin
      pr "IPC device not found after 5s, powering off.";
      Pid1.poweroff ()
    end;
    let port = Zephyr.Virtio_port.open_write ipc_name in
    Fun.protect
      ~finally:(fun () -> Zephyr.Virtio_port.close port)
      (fun () -> run_sim port)
  end
  else begin
    pr "Not running as PID 1 (demo mode)";
    pr "";
    pr "In a real VM, this would:";
    pr "  1. Mount /proc, /sys, /dev";
    pr "  2. Open /dev/virtio-ports/ipc";
    pr "  3. Propagate ISS orbit via SGP4";
    pr "  4. Stream continuous TM, HEALTH, EVR frames"
  end