"""
BBD202 Snake Scan Test - 20x20mm Area

Scans a 20mm x 20mm area centered at X=55, Y=37.5 in a snake pattern.
Row spacing: 0.5mm
Movement order: X+, Y+, X-, Y+ (snake pattern)
"""

import sys
import time
import argparse
from hardware.bbd202 import MotionController, MotorStatusBits


# Scan parameters
CENTER_X = 55.0      # mm
CENTER_Y = 37.5      # mm
SCAN_WIDTH = 20.0    # mm (X direction)
SCAN_HEIGHT = 20.0   # mm (Y direction)
ROW_SPACING = 0.5    # mm (Y step between rows)

# Calculated bounds
START_X = CENTER_X - SCAN_WIDTH / 2    # 45.0 mm
END_X = CENTER_X + SCAN_WIDTH / 2      # 65.0 mm
START_Y = CENTER_Y - SCAN_HEIGHT / 2   # 27.5 mm
END_Y = CENTER_Y + SCAN_HEIGHT / 2     # 47.5 mm


def snake_scan(mc: MotionController) -> bool:
    """
    Execute a snake scan pattern over the 20x20mm area.

    Pattern (X+, Y+, X-, Y+):
        Row 0: X=45 -> X=65 (X+)
        Step:  Y += 0.5 (Y+)
        Row 1: X=65 -> X=45 (X-)
        Step:  Y += 0.5 (Y+)
        ... repeat until Y reaches 47.5mm
    """
    print("\n" + "=" * 70)
    print("SNAKE SCAN TEST: 20mm x 20mm")
    print("=" * 70)
    print(f"Center:      X={CENTER_X:.1f}mm, Y={CENTER_Y:.1f}mm")
    print(f"Bounds:      X=[{START_X:.1f}, {END_X:.1f}]mm, Y=[{START_Y:.1f}, {END_Y:.1f}]mm")
    print(f"Row spacing: {ROW_SPACING}mm")
    print(f"Pattern:     X+, Y+, X-, Y+ (snake)")

    # Calculate number of rows
    num_rows = int(SCAN_HEIGHT / ROW_SPACING) + 1
    print(f"Total rows:  {num_rows}")

    # Stop automatic ACK to prevent interference with move commands
    # We'll send manual ACKs after each move completes
    mc.stop_status_ack()
    print("Using manual ACK mode for scan")

    # First, move to the starting position
    print(f"\n--- Moving to start position ({START_X:.1f}, {START_Y:.1f}) ---")
    mc.move_to_fast(x=START_X, y=START_Y)

    move_start = time.time()
    while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
        if time.time() - move_start > 30.0:
            print("** TIMEOUT moving to start position!")
            return False

    # Verify starting position
    mc.poll_positions()
    time.sleep(0.1)
    curr_x = mc.position_x
    curr_y = mc.position_y
    print(f"At start:    X={curr_x:.4f}mm, Y={curr_y:.4f}mm")

    # Generate snake path waypoints
    waypoints = []
    for row in range(num_rows):
        y_pos = START_Y + row * ROW_SPACING

        if row % 2 == 0:
            # Even row: move X+ (left to right)
            waypoints.append((END_X, y_pos, f"Row {row + 1}/{num_rows} X+ (Y={y_pos:.1f})"))
        else:
            # Odd row: move X- (right to left)
            waypoints.append((START_X, y_pos, f"Row {row + 1}/{num_rows} X- (Y={y_pos:.1f})"))

    print(f"\nTotal waypoints: {len(waypoints)}")
    print("-" * 70)

    all_passed = True
    total_distance = 0.0
    total_time = 0.0
    scan_start_time = time.time()

    # Execute the snake pattern
    for i, (target_x, target_y, description) in enumerate(waypoints):
        # Record start position for distance calculation
        mc.poll_positions()
        time.sleep(0.02)
        prev_x = mc.position_x
        prev_y = mc.position_y

        if prev_x is not None and prev_y is not None:
            distance = ((target_x - prev_x) ** 2 + (target_y - prev_y) ** 2) ** 0.5
            total_distance += distance

        # Clear any previous error
        mc.clear_last_error()

        # Issue move command
        move_start = time.time()
        mc.move_to_fast(x=target_x, y=target_y)

        # Wait for move to complete
        while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
            elapsed = time.time() - move_start

            # Timeout check
            if elapsed > 30.0:
                print(f"\n** TIMEOUT on {description} after 30s!")
                all_passed = False
                mc.clear_pending_moves()
                break

        # Move complete - send ACK to keep controller responsive
        mc.ack_status_update()

        elapsed = time.time() - move_start
        total_time += elapsed

        mc.poll_positions()
        time.sleep(0.02)
        final_x = mc.position_x
        final_y = mc.position_y

        if final_x is not None and final_y is not None:
            x_err = final_x - target_x
            y_err = final_y - target_y
            total_err = (x_err ** 2 + y_err ** 2) ** 0.5

            # Print progress (compact format)
            status = "OK" if total_err < 0.05 else "ERR"
            print(f"  {description:35} -> X={final_x:7.3f} Y={final_y:7.3f}  err={total_err * 1000:5.1f}um [{status}]")

            if total_err > 0.05:  # 50um tolerance
                all_passed = False

        # Check for errors after move
        err = mc.check_for_errors()
        if err:
            print(f"    ** Error: {err}")
            all_passed = False

        last_err = mc.last_error
        if last_err:
            print(f"    ** Last error: {last_err}")
            all_passed = False

    scan_elapsed = time.time() - scan_start_time

    # Return to start position
    print(f"\n--- Returning to start position ({START_X:.1f}, {START_Y:.1f}) ---")
    move_start = time.time()
    mc.move_to_fast(x=START_X, y=START_Y)

    while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
        if time.time() - move_start > 30.0:
            print("** TIMEOUT returning to start")
            mc.clear_pending_moves()
            break

    # Final summary
    print(f"\n{'=' * 70}")
    print("SNAKE SCAN SUMMARY")
    print(f"{'=' * 70}")

    mc.poll_positions()
    time.sleep(0.1)
    final_x = mc.position_x
    final_y = mc.position_y

    if final_x is not None and final_y is not None:
        return_x_err = abs(final_x - START_X)
        return_y_err = abs(final_y - START_Y)
        return_err = (return_x_err ** 2 + return_y_err ** 2) ** 0.5

        print(f"Scan area:        {SCAN_WIDTH:.1f}mm x {SCAN_HEIGHT:.1f}mm")
        print(f"Center:           X={CENTER_X:.1f}mm, Y={CENTER_Y:.1f}mm")
        print(f"Row spacing:      {ROW_SPACING}mm")
        print(f"Rows completed:   {num_rows}")
        print(f"Total distance:   {total_distance:.2f}mm")
        print(f"Scan time:        {scan_elapsed:.2f}s")
        print(f"Average speed:    {total_distance / scan_elapsed:.2f}mm/s")
        print(f"Return error:     {return_err * 1000:.1f}um (X={return_x_err * 1000:.1f}um, Y={return_y_err * 1000:.1f}um)")

        if return_err > 0.05:
            print(f"\n** FAIL: Return position error exceeds 50um")
            all_passed = False

    # Restart automatic ACK
    mc.start_status_ack()

    if all_passed:
        print(f"\n** PASS: Snake scan completed successfully **")
    else:
        print(f"\n** FAIL: Issues detected during snake scan **")

    return all_passed


def main():
    parser = argparse.ArgumentParser(description="BBD202 Snake Scan Test - 20x20mm Area")
    parser.add_argument("--auto-home", action="store_true",
                        help="Automatically home axes if not homed (no prompt)")
    args = parser.parse_args()

    print("=" * 70)
    print("BBD202 Snake Scan Test - 20x20mm Area")
    print("=" * 70)

    mc = None
    try:
        # Connect to controller
        print("\nConnecting to BBD202 controller...")
        mc = MotionController()
        mc.connect(enable_updates=True)
        print("Connected!")

        # Brief hardware info
        try:
            hw_info = mc.get_hw_info()
            print(f"\nHardware: {hw_info['model']} (S/N: {hw_info['serial_number']})")
            print(f"Firmware: {hw_info['firmware_version']}")
        except Exception as e:
            print(f"Could not get hardware info: {e}")

        # Check homing status
        print("\n--- Checking Homing Status ---")
        mc.request_status_update(mc.DEST_X_AXIS)
        mc.request_status_update(mc.DEST_Y_AXIS)
        time.sleep(0.2)
        mc.poll_status()
        time.sleep(0.1)

        x_homed = mc.is_homed_x
        y_homed = mc.is_homed_y

        print(f"X-axis homed: {x_homed}")
        print(f"Y-axis homed: {y_homed}")

        if not (x_homed and y_homed):
            print("\n** Axes not homed! **")

            # Check if we should auto-home or prompt
            should_home = args.auto_home
            if not should_home:
                try:
                    response = input("Home axes now? (y/n): ").strip().lower()
                    should_home = response == 'y'
                except EOFError:
                    print("Non-interactive mode detected. Use --auto-home flag.")
                    return 1

            if should_home:
                print("\nHoming X-axis...")
                x_result = mc.home_x_axis(timeout=30.0)
                print(f"X-axis: {'SUCCESS' if x_result else 'FAILED'}")

                print("\nHoming Y-axis...")
                y_result = mc.home_y_axis(timeout=30.0)
                print(f"Y-axis: {'SUCCESS' if y_result else 'FAILED'}")

                if not (x_result and y_result):
                    print("\n** Homing failed. Cannot continue. **")
                    return 1
            else:
                print("\nCannot run snake scan without homing. Exiting.")
                return 1

        # Run the snake scan
        success = snake_scan(mc)

        return 0 if success else 1

    except KeyboardInterrupt:
        print("\n\nTest interrupted by user")
        return 1
    except Exception as e:
        print(f"\n** ERROR: {e}")
        import traceback
        traceback.print_exc()
        return 1
    finally:
        if mc is not None:
            print("\nDisconnecting from controller...")
            mc.disconnect()
            print("Disconnected")


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