#!/usr/bin/env python3
"""
beam_watch.py : Lecture 5 opening demo. The wall does not move; the number does.

    python3 beam_watch.py                    # the beams around 0 deg in the scan frame
    python3 beam_watch.py --forward-deg 0 --half-deg 1
    python3 beam_watch.py --sim              # no robot: a simulated beam, 2.00 m, sigma 4 mm

Prints the forward reading on every scan, with its running mean, spread, and a
text histogram that fills in a bell shape. Ctrl-C to stop.
"""
import argparse, math, random, statistics, sys, time

class Stats:
    def __init__(self): self.n, self.mean, self.m2, self.vals = 0, 0.0, 0.0, []
    def add(self, x):
        self.n += 1; d = x - self.mean; self.mean += d / self.n; self.m2 += d * (x - self.mean); self.vals.append(x)
    @property
    def std(self): return math.sqrt(self.m2 / (self.n - 1)) if self.n > 1 else 0.0

def forward_reading(ranges, angle_min, inc, rmin, rmax, fwd, half):
    n = len(ranges)
    i0 = int(round((fwd - half - angle_min) / inc)); i1 = int(round((fwd + half - angle_min) / inc))
    vals = [ranges[i % n] for i in range(min(i0, i1), max(i0, i1) + 1)]
    good = [r for r in vals if math.isfinite(r) and rmin <= r <= rmax]
    return statistics.median(good) if good else None

def histogram(vals, mean, std, bins=17, width=46):
    if len(vals) < 5 or std <= 0: return []
    lo, hi = mean - 4 * std, mean + 4 * std; counts = [0] * bins
    for v in vals:
        k = int((v - lo) / (hi - lo) * bins)
        if 0 <= k < bins: counts[k] += 1
    top = max(counts) or 1
    return [f"  {1000 * (lo + (k + 0.5) * (hi - lo) / bins):8.1f} mm |{'#' * int(round(width * c / top))}" for k, c in enumerate(counts)]

def show(st, last):
    sys.stdout.write("\033[2J\033[H")
    print("beam_watch: one LiDAR beam at a wall that does not move\n")
    print(f"  scans {st.n:6d}   last {last:.4f} m   mean {st.mean:.4f} m   spread (sigma) {1000 * st.std:.1f} mm")
    print(f"  min {min(st.vals):.4f}   max {max(st.vals):.4f}   sigma of the MEAN {1000 * st.std / math.sqrt(max(st.n, 1)):.2f} mm\n")
    for line in histogram(st.vals[-2000:], st.mean, st.std): print(line)
    sys.stdout.flush()

def main():
    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("--forward-deg", type=float, default=0.0); ap.add_argument("--half-deg", type=float, default=1.0)
    ap.add_argument("--topic", default="/scan"); ap.add_argument("--sim", action="store_true")
    a, ros_args = ap.parse_known_args()
    st = Stats(); fwd, half = math.radians(a.forward_deg), math.radians(a.half_deg)
    if a.sim:
        rng = random.Random(1)
        try:
            while True:
                x = 2.0 + rng.gauss(0, 0.004); st.add(x)
                if st.n % 3 == 0: show(st, x)
                time.sleep(0.03)
        except KeyboardInterrupt:
            return 0
    import rclpy
    from rclpy.node import Node
    from rclpy.qos import qos_profile_sensor_data
    from sensor_msgs.msg import LaserScan
    class W(Node):
        def __init__(self):
            super().__init__("beam_watch"); self.t = 0.0
            self.create_subscription(LaserScan, a.topic, self.cb, qos_profile_sensor_data)
        def cb(self, m):
            r = forward_reading(m.ranges, m.angle_min, m.angle_increment, m.range_min, m.range_max, fwd, half)
            if r is None: return
            st.add(r)
            if time.time() - self.t > 0.3: self.t = time.time(); show(st, r)
    rclpy.init(args=[sys.argv[0]] + ros_args); node = W()
    try: rclpy.spin(node)
    except KeyboardInterrupt: pass
    finally:
        node.destroy_node()
        if rclpy.ok(): rclpy.shutdown()
    return 0

if __name__ == "__main__":
    sys.exit(main())