"""Solve planar UWB positions from one round of range measurements.

Run `python uwb_position.py --demo` for simulated data, or pass JSON Lines on stdin.
"""

import json
import sys


# ANCHOR: solver
import math


def locate_2d(anchors, ranges):
    """Return (x, y, range_rmse) in metres; anchors and tag share one height."""
    if not isinstance(ranges, dict) or len(ranges) < 3:
        raise ValueError("至少需要三个基站的距离")
    points = []
    for anchor_id, raw_range in ranges.items():
        if anchor_id not in anchors:
            raise ValueError(f"未知基站：{anchor_id}")
        if isinstance(raw_range, bool):
            raise ValueError("距离必须为正数")
        distance = float(raw_range)
        coordinate = tuple(float(v) for v in anchors[anchor_id])
        if len(coordinate) != 2 or not all(math.isfinite(v) for v in coordinate):
            raise ValueError("基站坐标必须是两个有限数值")
        if not math.isfinite(distance) or distance <= 0:
            raise ValueError("距离必须是有限正数")
        points.append((*coordinate, distance))

    x0, y0, r0 = points[0]
    rows = []
    for xi, yi, ri in points[1:]:
        dx, dy = xi - x0, yi - y0
        rows.append((2 * dx, 2 * dy, r0 * r0 - ri * ri + dx * dx + dy * dy))
    aa = sum(a * a for a, b, c in rows)
    ab = sum(a * b for a, b, c in rows)
    bb = sum(b * b for a, b, c in rows)
    ac = sum(a * c for a, b, c in rows)
    bc = sum(b * c for a, b, c in rows)
    determinant = aa * bb - ab * ab
    if determinant <= 1e-12 * max(aa * bb, 1.0):
        raise ValueError("基站共线或布置退化，无法进行二维定位")
    x = x0 + (ac * bb - bc * ab) / determinant
    y = y0 + (bc * aa - ac * ab) / determinant
    rmse = math.sqrt(sum((math.hypot(x - xi, y - yi) - ri) ** 2
                         for xi, yi, ri in points) / len(points))
    return x, y, rmse
# ANCHOR_END: solver


ANCHORS = {
    "A0": (0.0, 0.0),
    "A1": (6.0, 0.0),
    "A2": (6.0, 4.0),
    "A3": (0.0, 4.0),
}


def process_frame(frame):
    if not isinstance(frame, dict):
        raise ValueError("输入必须为 JSON 对象")
    x, y, rmse = locate_2d(ANCHORS, frame["ranges"])
    return {
        "tag_id": frame["tag_id"],
        "round_id": frame["round_id"],
        "x_m": round(x, 4),
        "y_m": round(y, 4),
        "range_rmse_m": round(rmse, 4),
    }


def main():
    if sys.argv[1:] == ["--demo"]:
        position = (2.0, 1.5)
        frame = {
            "tag_id": "T1", "round_id": 1,
            "ranges": {key: math.dist(position, coordinate)
                       for key, coordinate in ANCHORS.items()},
        }
        print(json.dumps(process_frame(frame), ensure_ascii=False, allow_nan=False))
        return 0
    if sys.argv[1:]:
        raise SystemExit("用法：python uwb_position.py [--demo]，或从标准输入读取 JSON Lines")
    failures = 0
    for line_number, line in enumerate(sys.stdin, 1):
        if not line.strip():
            continue
        try:
            result = process_frame(json.loads(line))
            print(json.dumps(result, ensure_ascii=False, allow_nan=False))
        except (ValueError, TypeError, KeyError) as error:
            failures += 1
            print(f"第 {line_number} 行处理失败：{error}", file=sys.stderr)
    return 1 if failures else 0


if __name__ == "__main__":
    raise SystemExit(main())