#!/usr/bin/env python3
"""
Planet Security MAVLink wrapper

Wraps every outgoing MAVLink command with a Planet Defense signature
header, and verifies every incoming command before passing to flight controller.

Compatible with PX4, ArduPilot, and any vehicle speaking MAVLink2.

Architecture
------------
  GCS (QGroundControl)
       │  COMMAND_LONG / COMMAND_INT
       ▼
  PlanetMavlinkWrapper.sign_outbound(msg)   ── Defense API ──> +Planet-Signature
       │
       ▼
  Pixhawk / drone FC (any vehicle radio link)
       │
       ▼
  PlanetMavlinkWrapper.verify_inbound(msg)  ── Defense API ──> reject if Δ violation,
                                                                replay, hijack, MITM

Usage
-----
    from pymavlink import mavutil
    from planet_mavlink_wrapper import PlanetMavlinkWrapper

    conn = mavutil.mavlink_connection("udp:0.0.0.0:14550")
    pm = PlanetMavlinkWrapper(
        base_url="https://planet.winnerbrothers.org",
        secret_key="sk_...",
        gcs_device_id="dev_...",
        uav_device_id="dev_...",
    )
    pm.connect()

    while True:
        msg = conn.recv_match(blocking=True)
        if not msg:
            continue
        if msg.get_type() == "COMMAND_LONG":
            payload = json.dumps(msg.to_dict(), sort_keys=True)
            ok = pm.verify_inbound(payload)
            if not ok:
                print(f"[!] Rejected by Planet: {pm.last_reject_reason}")
                continue
            # ... pass to FC ...
"""
import json
import sys
import os

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)) + "/../python")
from planet_defense_sdk import PlanetDefense  # noqa: E402


class PlanetMavlinkWrapper:
    def __init__(
        self, *,
        base_url: str, secret_key: str,
        gcs_device_id: str, uav_device_id: str,
        environment: str = "field",
    ):
        self.pd = PlanetDefense(base_url, secret_key)
        self.gcs_device_id = gcs_device_id
        self.uav_device_id = uav_device_id
        self.environment = environment
        self.session = None
        self.gcs = None
        self.uav = None
        self.last_reject_reason = None

    def connect(self) -> None:
        """Establish 3-Round handshake between GCS and UAV."""
        self.gcs = {"device": {"deviceId": self.gcs_device_id}}
        self.uav = {"device": {"deviceId": self.uav_device_id}}
        self.session = self.pd.handshake(initiator=self.gcs, responder=self.uav)

    def sign_outbound(self, mavlink_payload: str) -> dict:
        """Sign a MAVLink command payload (JSON-serialized) for transmission."""
        if not self.session:
            raise RuntimeError("call connect() first")
        return self.pd.sign_command(
            self.session, sender=self.gcs, command=mavlink_payload,
        )

    def verify_inbound(self, signed_bundle: dict) -> bool:
        """Verify a Planet-signed MAVLink bundle. Returns True if valid."""
        result = self.pd.verify_command(signed_bundle)
        if not result.get("valid"):
            self.last_reject_reason = result.get("reason")
            return False
        return True

    def heartbeat(self) -> dict:
        return self.pd.heartbeat(device_id=self.uav_device_id)


# ─────────────────────────────────────────────────────────────────────────────
# Standalone demo: simulate GCS → UAV TAKEOFF + replay attack
# ─────────────────────────────────────────────────────────────────────────────


def _demo(base_url: str, secret_key: str) -> None:
    print(f"[*] Connecting: {base_url}")
    pd = PlanetDefense(base_url, secret_key)

    gcs = pd.keygen(callsign="GCS-MAV", classification="gcs", environment="field")
    uav = pd.keygen(callsign="EAGLE-MAV", classification="uav", environment="field")
    pm = PlanetMavlinkWrapper(
        base_url=base_url, secret_key=secret_key,
        gcs_device_id=gcs["device"]["deviceId"],
        uav_device_id=uav["device"]["deviceId"],
        environment="field",
    )
    pm.connect()
    print(f"[*] Session deltaMs={pm.session['deltaMs']}")

    # Simulate MAV_CMD_NAV_TAKEOFF
    payload = json.dumps({
        "command": 22,           # MAV_CMD_NAV_TAKEOFF
        "param7": 50.0,          # altitude meters
        "target_system": 1,
        "target_component": 1,
    })
    signed = pm.sign_outbound(payload)
    print(f"[*] Signed TAKEOFF cmdId={signed['commandId']}")

    ok = pm.verify_inbound(signed)
    print(f"    [{'OK' if ok else 'NO'}] verify={ok}")

    print("[*] Replay attack...")
    ok2 = pm.verify_inbound(signed)
    print(f"    [{'OK rejected' if not ok2 else 'NO ACCEPTED'}] verify={ok2} reason={pm.last_reject_reason}")


if __name__ == "__main__":
    import argparse
    p = argparse.ArgumentParser()
    p.add_argument("--base-url", default="http://localhost:3000")
    p.add_argument("--secret-key", required=True)
    args = p.parse_args()
    _demo(args.base_url, args.secret_key)