import requests
import serial
import time
import sys
import re

# ================= CONFIGURATION =================
CAMERA_IP = "192.168.0.10"
ARDUINO_PORT = "/dev/ttyUSB0"
BAUD_RATE = 9600

# Motion Control
ROTATION_SPEED_DPS = 45.0
ROTATION_STEP_SIZE = 15.0
TOTAL_TARGET_DEGREES = 360.0

# Timing
PHOTO_TO_MOVE_DELAY = 8.0  # Wait after photo before rotation starts
SERIAL_TIMEOUT = 10.0      # Increased timeout for long moves
# ================================================

def emergency_shutdown(error_msg, ser=None):
    """
    Failsafe routine: Attempts to switch camera to play mode and exits.
    """
    print(f"\n[!] ERROR DETECTED: {error_msg}")
    print("Initiating failsafe: Switching camera back to default state...")
    
    try:
        requests.get(f"http://{CAMERA_IP}/switch_cammode.cgi?mode=play", timeout=5)
        print("Camera switched to play mode.")
    except Exception as e:
        print(f"Failed to switch camera mode during shutdown: {e}")

    if ser:
        try:
            ser.close()
            print("Serial connection closed.")
        except:
            pass

    print("System halted.")
    sys.exit(1)

def send_arduino_command(ser, command):
    """
    Sends a command to the Arduino. 
    Does NOT wait for a response here to keep the flow simple, 
    but the response is consumed in the main loop.
    """
    try:
        ser.write(f"{command}\n".encode())
        # Small delay to ensure Arduino receives command before next action
        time.sleep(0.1)
    except Exception as e:
        raise Exception(f"Failed to send command '{command}': {e}")

def wait_for_move_completion(ser, expected_angle, speed_dps):
    """
    Waits until the Arduino reports 'Steps Left: 0'.
    Falls back to a calculated timeout if serial parsing fails.
    """
    # Calculate expected time for the move as a safety fallback
    # angle / speed = seconds. Add 10% buffer.
    expected_time = (expected_angle / speed_dps) * 1.2
    start_time = time.time()
    
    print(f"Waiting for movement ({expected_angle}°) to complete...")
    
    while True:
        if ser.in_waiting > 0:
            try:
                line = ser.readline().decode('utf-8').strip()
                if not line:
                    continue
                
                # Check for "Steps Left: 0" in the Arduino status output
                # Your Arduino prints: " | Steps Left: 0"
                if "Steps Left: 0" in line:
                    print("Movement complete (Steps Left: 0 detected).")
                    return True
                
                # Optional: Debug print if needed
                # print(f"Arduino: {line}")
            except Exception as e:
                print(f"Warning: Error reading serial: {e}")

        # Fallback timeout
        elapsed = time.time() - start_time
        if elapsed > max(expected_time + 5, 30): # Max 30s or calculated time + 5s
            print(f"Timeout reached ({elapsed:.1f}s). Assuming move finished.")
            return False

        time.sleep(0.1) # Prevent CPU spinning

def main():
    ser = None
    try:
        # 1. Initialize Serial Connection (Persistent)
        print("Opening serial port...")
        ser = serial.Serial(ARDUINO_PORT, BAUD_RATE, timeout=1)
        time.sleep(2) # Wait for Arduino to reset and boot

        # Flush any garbage data from boot
        ser.reset_input_buffer()
        ser.reset_output_buffer()

        # 2. Initialize Table Parameters
        print("Initializing table parameters...")
        send_arduino_command(ser, f"s{ROTATION_SPEED_DPS}")
        time.sleep(0.5)
        send_arduino_command(ser, "m1") # Set to Manual mode (controlled by script)
        time.sleep(0.5)

        # 3. Prepare Camera State
        print("Switching camera to shutter mode...")
        requests.get(f"http://{CAMERA_IP}/switch_cammode.cgi?mode=shutter", timeout=5)
        print("System ready.")

        current_angle = 0.0
        total_steps = int(TOTAL_TARGET_DEGREES / ROTATION_STEP_SIZE)
        
        # Ensure we cover the full circle if 360 is the target
        # If 360/15 = 24 steps, we do 24 moves. 
        # The loop runs while current_angle < TOTAL (0 to 345), then we do one last check?
        # Actually, simpler: Loop for exactly the number of steps needed.
        
        steps_taken = 0
        while steps_taken <= total_steps:
            # A. Capture Image
            angle_label = int(current_angle)
            print(f"\n--- Step {steps_taken + 1}/{total_steps + 1} ---")
            print(f"Capturing image at {angle_label}°...")
            
            try:
                # 1. Push Shutter (Start exposure)
                req1 = requests.get(f"http://{CAMERA_IP}/exec_shutter.cgi?type=1st2ndpush", timeout=10)
                time.sleep(0.1)
                
                # 2. Release Shutter (End exposure) - CRITICAL STEP
                # The user specified this command is required to actually take the photo
                req2 = requests.get(f"http://{CAMERA_IP}/exec_shutter.cgi?com=2nd1strelease", timeout=10)
                
                if req1.status_code != 200 or req2.status_code != 200:
                    raise Exception(f"Camera returned status {req1.status_code} / {req2.status_code}")
                    
                print("Image captured successfully.")
            except Exception as e:
                raise Exception(f"Camera shutter sequence failed: {e}")

            # If this is the last step, we don't need to move or wait
            if steps_taken == total_steps:
                print("All steps completed.")
                break

            # B. Wait for camera processing
            print(f"Waiting {PHOTO_TO_MOVE_DELAY}s for camera processing...")
            time.sleep(PHOTO_TO_MOVE_DELAY)

            # C. Rotate Table
            print(f"Rotating {ROTATION_STEP_SIZE}°...")
            send_arduino_command(ser, f"a{ROTATION_STEP_SIZE}")
            
            # D. Wait for movement to finish
            # We pass the step size and speed to calculate a safe wait time
            wait_for_move_completion(ser, ROTATION_STEP_SIZE, ROTATION_SPEED_DPS)
            
            current_angle += ROTATION_STEP_SIZE
            steps_taken += 1
            print(f"Progress: {int(current_angle)}/{TOTAL_TARGET_DEGREES} degrees.")

        print("\n[SUCCESS] Full rotation completed successfully.")

    except Exception as e:
        emergency_shutdown(str(e), ser)
    finally:
        if ser and ser.is_open:
            ser.close()

if __name__ == "__main__":
    main()