///|
/// Standard SHA-256 round constants `K[0..63]` (first 32 bits of the fractional parts of cube roots of first 64 primes).
let sha256_k : FixedArray[UInt32] = [
  0x428a2f98U, 0x71374491U, 0xb5c0fbcfU, 0xe9b5dba5U, 0x3956c25bU, 0x59f111f1U, 0x923f82a4U,
  0xab1c5ed5U, 0xd807aa98U, 0x12835b01U, 0x243185beU, 0x550c7dc3U, 0x72be5d74U, 0x80deb1feU,
  0x9bdc06a7U, 0xc19bf174U, 0xe49b69c1U, 0xefbe4786U, 0x0fc19dc6U, 0x240ca1ccU, 0x2de92c6fU,
  0x4a7484aaU, 0x5cb0a9dcU, 0x76f988daU, 0x983e5152U, 0xa831c66dU, 0xb00327c8U, 0xbf597fc7U,
  0xc6e00bf3U, 0xd5a79147U, 0x06ca6351U, 0x14292967U, 0x27b70a85U, 0x2e1b2138U, 0x4d2c6dfcU,
  0x53380d13U, 0x650a7354U, 0x766a0abbU, 0x81c2c92eU, 0x92722c85U, 0xa2bfe8a1U, 0xa81a664bU,
  0xc24b8b70U, 0xc76c51a3U, 0xd192e819U, 0xd6990624U, 0xf40e3585U, 0x106aa070U, 0x19a4c116U,
  0x1e376c08U, 0x2748774cU, 0x34b0bcb5U, 0x391c0cb3U, 0x4ed8aa4aU, 0x5b9cca4fU, 0x682e6ff3U,
  0x748f82eeU, 0x78a5636fU, 0x84c87814U, 0x8cc70208U, 0x90befffaU, 0xa4506cebU, 0xbef9a3f7U,
  0xc67178f2U,
]

///|
fn rotr32(x : UInt32, n : Int32) -> UInt32 {
  (x >> n) | (x << (32 - n))
}

///|
/// Compute the 32-byte SHA-256 digest of `input` (FIPS 180-4).
pub fn sha256_digest(input : Bytes) -> FixedArray[Byte] {
  let mut h0 = 0x6a09e667U
  let mut h1 = 0xbb67ae85U
  let mut h2 = 0x3c6ef372U
  let mut h3 = 0xa54ff53aU
  let mut h4 = 0x510e527fU
  let mut h5 = 0x9b05688cU
  let mut h6 = 0x1f83d9abU
  let mut h7 = 0x5be0cd19U

  let msg_len = input.length()
  let bit_len = msg_len.to_uint64() * 8UL
  let padded : Array[Byte] = Array(capacity=msg_len + 72)
  for i = 0; i < msg_len; i = i + 1 {
    padded.push(input[i])
  }
  padded.push(b'\x80')
  while padded.length() % 64 != 56 {
    padded.push(b'\x00')
  }
  for shift = 56; shift >= 0; shift = shift - 8 {
    padded.push(((bit_len >> shift) & 0xFFUL).to_byte())
  }

  let w = FixedArray::make(64, 0U)
  let num_blocks = padded.length() / 64
  for blk = 0; blk < num_blocks; blk = blk + 1 {
    let base = blk * 64
    for i = 0; i < 16; i = i + 1 {
      let idx = base + i * 4
      let b0 = padded[idx].to_uint()
      let b1 = padded[idx + 1].to_uint()
      let b2 = padded[idx + 2].to_uint()
      let b3 = padded[idx + 3].to_uint()
      w[i] = (b0 << 24) | (b1 << 16) | (b2 << 8) | b3
    }
    for i = 16; i < 64; i = i + 1 {
      let s0 = rotr32(w[i - 15], 7) ^ rotr32(w[i - 15], 18) ^ (w[i - 15] >> 3)
      let s1 = rotr32(w[i - 2], 17) ^ rotr32(w[i - 2], 19) ^ (w[i - 2] >> 10)
      w[i] = w[i - 16] + s0 + w[i - 7] + s1
    }

    let mut a = h0
    let mut b = h1
    let mut c = h2
    let mut d = h3
    let mut e = h4
    let mut f = h5
    let mut g = h6
    let mut h = h7

    for i = 0; i < 64; i = i + 1 {
      let s1 = rotr32(e, 6) ^ rotr32(e, 11) ^ rotr32(e, 25)
      let ch = (e & f) ^ (e.lnot() & g)
      let temp1 = h + s1 + ch + sha256_k[i] + w[i]
      let s0 = rotr32(a, 2) ^ rotr32(a, 13) ^ rotr32(a, 22)
      let maj = (a & b) ^ (a & c) ^ (b & c)
      let temp2 = s0 + maj

      h = g
      g = f
      f = e
      e = d + temp1
      d = c
      c = b
      b = a
      a = temp1 + temp2
    }

    h0 = h0 + a
    h1 = h1 + b
    h2 = h2 + c
    h3 = h3 + d
    h4 = h4 + e
    h5 = h5 + f
    h6 = h6 + g
    h7 = h7 + h
  }

  let out = FixedArray::make(32, b'\x00')
  let words : FixedArray[UInt32] = [h0, h1, h2, h3, h4, h5, h6, h7]
  for i = 0; i < 8; i = i + 1 {
    let v = words[i]
    out[i * 4] = ((v >> 24) & 0xFFU).to_byte()
    out[i * 4 + 1] = ((v >> 16) & 0xFFU).to_byte()
    out[i * 4 + 2] = ((v >> 8) & 0xFFU).to_byte()
    out[i * 4 + 3] = (v & 0xFFU).to_byte()
  }
  out
}

///|
/// Stateful MAVLink v2 packet signing and verification context.
///
/// Per the MAVLink v2 Packet Signing specification:
/// - `secret_key` is a 32-byte shared secret.
/// - `link_id` identifies the communication link (`0..255`).
/// - `timestamp` is a 48-bit monotonically increasing counter in 10-microsecond units since 2015-01-01.
/// - Signature is the first 6 bytes of `SHA256(secret_key || frame_without_sig || link_id || timestamp_le6)`.
pub(all) struct SigningContext {
  secret_key : FixedArray[Byte]
  link_id : Byte
  mut timestamp : UInt64
} derive(Eq, Debug)

///|
pub extend SigningContext with Eq::{not_equal, equal}

///|
pub extend SigningContext with @debug.Debug::{to_repr}

///|
/// Create a new `SigningContext` with a 32-byte secret key, `link_id`, and initial 48-bit `timestamp`.
pub fn SigningContext::new(
  secret_key : Bytes,
  link_id~ : Byte = b'\x00',
  initial_timestamp~ : UInt64 = 1UL,
) -> Result[SigningContext, MavError] {
  if secret_key.length() != 32 {
    return Err(
      SignatureInvalid("MAVLink v2 signing secret key must be exactly 32 bytes"),
    )
  }
  let key_arr = FixedArray::make(32, b'\x00')
  for i = 0; i < 32; i = i + 1 {
    key_arr[i] = secret_key[i]
  }
  Ok({
    secret_key: key_arr,
    link_id,
    timestamp: initial_timestamp & 0xFFFFFFFFFFFFUL,
  })
}

///|
/// Compute the 6-byte truncated SHA-256 signature for the given unsigned frame bytes
/// (`STX` through `CRC`, inclusive), `link_id`, and 48-bit `timestamp`.
pub fn compute_v2_signature_bytes(
  secret_key : FixedArray[Byte],
  unsigned_frame_bytes : Bytes,
  link_id : Byte,
  timestamp : UInt64,
) -> FixedArray[Byte] {
  let buf : Array[Byte] = Array(capacity=32 + unsigned_frame_bytes.length() + 7)
  for i = 0; i < 32; i = i + 1 {
    buf.push(secret_key[i])
  }
  for i = 0; i < unsigned_frame_bytes.length(); i = i + 1 {
    buf.push(unsigned_frame_bytes[i])
  }
  buf.push(link_id)
  for shift = 0; shift < 48; shift = shift + 8 {
    buf.push(((timestamp >> shift) & 0xFFUL).to_byte())
  }
  let digest = sha256_digest(Bytes::from_array(buf))
  let sig = FixedArray::make(6, b'\x00')
  for i = 0; i < 6; i = i + 1 {
    sig[i] = digest[i]
  }
  sig
}

///|
/// Generate the next `MavSignature` for an outgoing MAVLink v2 frame and increment the context timestamp.
pub fn SigningContext::sign_frame_bytes(
  self : SigningContext,
  unsigned_frame_bytes : Bytes,
) -> MavSignature {
  let ts = self.timestamp & 0xFFFFFFFFFFFFUL
  let sig = compute_v2_signature_bytes(
    self.secret_key,
    unsigned_frame_bytes,
    self.link_id,
    ts,
  )
  self.timestamp = (ts + 1UL) & 0xFFFFFFFFFFFFUL
  { link_id: self.link_id, timestamp: ts, signature: sig, }
}

///|
/// Verify an incoming MAVLink v2 frame signature and enforce monotonic timestamp replay protection.
pub fn SigningContext::verify_frame_signature(
  self : SigningContext,
  unsigned_frame_bytes : Bytes,
  sig : MavSignature,
  enforce_replay_protection~ : Bool = true,
) -> Result[Unit, MavError] {
  if enforce_replay_protection && sig.timestamp < self.timestamp {
    return Err(
      SignatureInvalid(
        "Replay detected: incoming timestamp is older than last accepted timestamp",
      ),
    )
  }
  let expected = compute_v2_signature_bytes(
    self.secret_key,
    unsigned_frame_bytes,
    sig.link_id,
    sig.timestamp,
  )
  for i = 0; i < 6; i = i + 1 {
    if expected[i] != sig.signature[i] {
      return Err(SignatureInvalid("Packet signature digest mismatch"))
    }
  }
  if enforce_replay_protection && sig.timestamp >= self.timestamp {
    self.timestamp = (sig.timestamp + 1UL) & 0xFFFFFFFFFFFFUL
  }
  Ok(())
}