namespace KWorld
open Stdlib
open UnityPeripherals.Lowlevel

type 'a expr = 'a Asyncseq.expr

type coord =
  { roll: float; pitch: float }

type coord_builder() =
  interface Serializable<coord, System.Double expr * System.Double expr> with
    override __.lit ((roll, pitch)) =
      ("%d", "%d"), Varargs.make2 (Varargs.double_to_int 100 roll) (Varargs.double_to_int 100 pitch)
  interface Evaluate<coord> with
    override __.eval o =
      let roll, pitch =
        int <| o.["Item1"].GetValue<string>(),
        int <| o.["Item2"].GetValue<string>()
      { roll = float roll / 100.0; pitch = float pitch / 100.0 }

namespace KWorld
open UnityPeripherals
open UnityPeripherals.Lowlevel

type imu() =
  interface wireable' with
    override val id = 0x68

open Stdlib (* NOTE(kinten) redundant opening for better type name presentation *)
open Stdlib.Asyncseq (* NOTE(kinten) redundant opening for better type name presentation *)
open Stdlib.Syntax
open UnityPeripherals
open UnityPeripherals.Lowlevel
open UnityPeripherals.Lowlevel.Math
open UnityPeripherals.Lowlevel.Math.Double.Syntax
#nowarn "25"

type editor_app(o) =
  inherit editor_os_1<coord, double expr * double expr, coord_builder>(o)

  let make_angle vx vy vz =
    let parse x' y' z =
      Double.atan (x' /. Double.sqrt (y' *. y' +. z *. z))
      |> Double.mul (o.lit (180.0 / 3.14))
    let d u = Unsafe.cast Caster.double u
    let pitch = parse (d vx) (d vy) (d vz)
    let roll  = parse (d vy) (d vx) (d vz)
    pitch, roll

  let consume dev = o.async {
    let fix (x_sign, x_val, offset) =
      Bitvec.sll (o.int 8) x_sign
      |> Bitvec.or' x_val
      |> Int.add (o.int offset)
    do! Wire.Buf_write.with_trnms dev @@ fun to_dev ->
      Wire.Buf_write.byte to_dev (o.int 0x3B)
    let! accx_sign :: accx_val
      :: accy_sign :: accy_val
      :: accz_sign :: accz_val :: _ =
      Wire.Request_for.n_from 14 dev
    let ret = Tup3.map fix (
      (accx_sign, accx_val, -250),
      (accy_sign, accy_val, 36),
      (accz_sign, accy_val, 1200))
    return o.tuple ret }

  let rec seq' env () = o.asyncseq {
    let mpu, clock = env
    let ( .. ) func value = o.async {
      let delay = Async.sleep clock (o.lit 150)
      let! () = delay in let! x = func value
      return o.tuple x }
    let! a, b, c = (..) consume mpu
    yield make_angle a b c
    yield! seq' env
  }

  override __.seq with get env =
    let clock = Stdenv.clock env
    UnityPeripherals.Seq.append (base.seq env) @@
    Wire.with_open_1<imu, _, _> @@ fun dev ->
    seq' (dev, clock) ()

open UnityPeripherals.System.Threading.Tasks

type editor_app with
  // TODO(kinten) combine more here
  static member val all =
    let make o = new editor_app (o)
    Task.Object.task make
    |> Task.Object.finish