#!/usr/bin/env pybricks-micropython

# Import modules
import urandom
from mindsensorsPYB import EV3RFid
import urequests as requests

from pybricks.ev3devices import ColorSensor, Motor, UltrasonicSensor
from pybricks.hubs import EV3Brick
from pybricks.parameters import Port, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import StopWatch, wait

# Initialize program constants
SERVER = "http://192.168.0.129:5000"
DEVICE_NAME = "JEEP"

# Mapping of tag ID to RFID ID code in orchestration
RFID_MAP = {
    "1549617941": "01",
    "1556369429": "02",
    "1547589141": "03",
    "1556830997": "04",
    "1549230357": "05",

    "2883759443": "01",
    "2885645267": "02",
    "327324843": "03",
    "492451459": "04",
    "493143587": "05"
}

POLL_INTERVAL = 0.05 # s
HEARTBEAT_INTERVAL = 2.0 # s

RATE_HIGH = 75 # deg/s
RATE_LOW = 20
RATE_MEDIUM = 50

SPEED_HIGH = 100 # mm/s
SPEED_LOW = 20
SPEED_MEDIUM = 50

THRESHOLD_OBSTACLE = 200 # mm
OBSTACLE_TIMEOUT = 3.0 # s

TIME_WAIT = 10 # ms

# -------------------
# SERVER CLIENT class
# -------------------
class ServerClient:
    # Initialize self
    def __init__(self, server, device):

        print("[SERVERCLIENT] Initializing")
        self.server = server
        self.device = device

    # Send event
    def send_event(self, kind, detail):
        # Populate JSON payload
        payload = {
            "device": self.device,
            "type": kind,
            "value": detail
        }
        # Try sending JSON event to /event endpoint on Flask server
        try:
            r = requests.post(self.server + "/event", json=payload, headers={"Connection": "close"}
)
            r.close()
            print("[EVENT]", payload)

        except Exception as e:
            print("[HTTP] Sending event failed with error '", e, "'")
            wait(200)

    # Receive command
    def get_command(self):
        # Try receiving JSON command from /sent_command/ endpoint on Flask server
        try:
            r = requests.get(self.server + "/sent_command/" + self.device, headers={"Connection": "close"}
)
            data = r.json()
            r.close()

            return data.get("command sent")

        except Exception as e:
            print("[HTTP] Receiving command failed with error '", e, "'")
            wait(200)
            return None

# --------------------
# HARDWARE LAYER class
# --------------------
class Hardware:
    # Initialize self
    def __init__(self):

        print("[HARDWARE] Initializing")
        # MODIFY TO SUIT REQUIRED HARDWARE
        self.ev3 = EV3Brick()
        # Initialize robot constants
        self.axle_track = 135 # mm distance between middle of tire contact
        self.wheel_diameter = 55.5 # mm
        # Initialize motors
        self.motor_left = Motor(Port.B)
        self.motor_right = Motor(Port.C)
        self.drive_base = DriveBase(self.motor_left, self.motor_right, self.wheel_diameter, self.axle_track)
        # Sensors
        self.sensor_colour_left = ColorSensor(Port.S2)
        self.sensor_colour_right = ColorSensor(Port.S3)
        self.sensor_rfid = EV3RFid(Port.S1, 0x22)
        self.sensor_us = UltrasonicSensor(Port.S4)
        
        self.last_uid = None

    def forward(self):
        print("[HW] Running command forward")
        self.drive_base.drive(SPEED_MEDIUM, 0)

    def backward(self):
        print("[HW] Running command backward")
        self.drive_base.drive(-SPEED_MEDIUM, 0)

    def start(self):
        print("[HW] Running command start")
        self.drive_base.drive(SPEED_MEDIUM, 0)
        
    def stop(self):
        print("[HW] Running command stop")
        self.drive_base.stop(Stop.BRAKE)

    def beep(self):
        print("[HW] Running command beep")
        self.ev3.speaker.beep()

# ------------------
# DEVICE LOGIC class
# ------------------
class Device:
    # Initialize self
    def __init__(self, name, server):

        self.name = name
        self.server = ServerClient(server, name)
        self.hw = Hardware()
        self.clock = StopWatch()
        self.last_heartbeat = 0
        self.start_received = False
        # Line-follow calibration
        self.threshold = 50  # Default overwritten by calibrate() function
        # Obstacle handling
        self.obstacle_start_time = None
        self.avoiding = False
        
        self.first_uid = True
        
    # Calibrate colour sensors
    def calibrate(self):
        # Declare function variables        
        ev3 = self.hw.ev3
        left = self.hw.sensor_colour_left
        right = self.hw.sensor_colour_right
        
        # Calibrate
        ev3.speaker.say('Press any button')
        self.wait_for_button()
        ev3.speaker.say('Calibrating black')
        wait(500)
        left_black = left.reflection()
        right_black = right.reflection()
        
        ev3.speaker.say('Press any button')
        self.wait_for_button()
        ev3.speaker.say('Calibrating white')
        wait(500)
        left_white = left.reflection()
        right_white = right.reflection()

        # Calculate variables
        if left_white >= right_white:
            min_white = right_white
        else:
            min_white = left_white

        if left_black >= right_black:
            max_black = left_black
        else:
            max_black = right_black

        self.threshold = ( min_white + max_black ) / 2

        print('[CAL] Left white:', left_white, 'Right white:', right_white, 'White min:', min_white)
        print('[CAL] Left black:', left_black, 'Right black:', right_black, 'Black max:', max_black)
        print('[CAL] Threshold :', self.threshold)

    # Find RFID tags
    def check_rfid(self):

        uid = self.hw.sensor_rfid.readUID()
        uid_str = None
        # Normal behaviour after startup
        if uid and uid != self.hw.last_uid:
            self.hw.last_uid = uid
            
            # TCP event
            uid_str = str(uid).strip()
            mapped = RFID_MAP.get(uid_str, "00")
            # Ignore stale UID
            if self.first_uid and mapped != "01":
                self.first_uid = False
                return
            
            self.server.send_event("RFID", mapped)
            print("[RFID] UID:", uid, "Mapped:", mapped)

        if uid is None:
            self.last_uid = None

    # Follow the line
    def follow_line(self):
        # MODIFY TO SUIT REQUIRED CODE
        # Declare function variables
        intensity_left = self.hw.sensor_colour_left.reflection()
        intensity_right = self.hw.sensor_colour_right.reflection()

        if intensity_left >= self.threshold and intensity_right >= self.threshold:
            # Both see white, move forward
            self.hw.drive_base.drive(SPEED_MEDIUM, 0)
        elif intensity_left < self.threshold and intensity_right >= self.threshold:
            # Left sensor sees black, turn left medium
            self.hw.drive_base.drive(SPEED_MEDIUM, -RATE_MEDIUM)
        elif intensity_left >= self.threshold and intensity_right < self.threshold:
            # Right sensor sees black, turn right medium
            self.hw.drive_base.drive(SPEED_MEDIUM, RATE_MEDIUM)
        elif intensity_left < self.threshold and intensity_right < self.threshold:
            # Both see 'black', move forward slowly
            self.hw.drive_base.drive(SPEED_LOW, 0)
        # Wait to avoid excessive loop execution
        wait(TIME_WAIT)
                
    # Handle command sent by Flask server
    def handle_command(self, cmd):
        
        print("[CMD] Receiving command '", cmd, "'")
        # MODIFY TO SUIT REQUIRED COMMANDS
        if cmd == "forward":
            self.hw.forward()

        elif cmd == "backward":
            self.hw.backward()

        elif cmd == "start":
            # If command is 'start', allow code to proceed to periodic()                    
            self.start_received = True
            self.server.send_event("EVENT", "04")
            
        elif cmd == "stop":
            self.hw.stop()
            self.server.send_event("EVENT", "02")

        elif cmd == "beep":
            self.hw.beep()

        else:
            print("[CMD] Receiving unknown command '", cmd, "'")
 
    # Detect & avoid obstacle
    def handle_obstacle(self):

        distance = self.hw.sensor_us.distance()
        now = self.clock.time() / 1000  # s

        if distance is None:
            return False  # Treat as no obstacle

        if distance < THRESHOLD_OBSTACLE:
            msg = "Obstacle:" + str(distance) + "mm"
            self.server.send_event("STATUS", msg)
            
            if self.obstacle_start_time is None:
                self.obstacle_start_time = now
                print("[OBS] Obstacle detected. Waiting ...")
            self.hw.stop()

            # If obstacle persists too long, try to drive around
            if not self.avoiding and (now - self.obstacle_start_time) > OBSTACLE_TIMEOUT:
                print("[OBS] Timeout. Attempting to drive around ...")
                self.avoiding = True
                # Simple avoidance: back up, turn, move forward a bit
                self.hw.drive_base.straight(-100)
                self.hw.drive_base.turn(urandom.choice([-60, 60]))
                self.hw.drive_base.straight(150)
                # Reset and let line-follow re-acquire
                self.obstacle_start_time = None
                self.avoiding = False
            return True
        else:
            self.obstacle_start_time = None
            return False
            
    # Send status 'alive' to Flask server
    def heartbeat(self):
        self.server.send_event("STATUS", "alive")
            
    # Define functions run periodically
    def periodic(self):
        # MODIFY TO SUIT REQUIRED CODE
        # Find RFID tags
        self.check_rfid()
        
        # Handle obstacle 
        if self.handle_obstacle():
            return

        # Follow line
        self.follow_line()

    # Send event 'ready' to Flask server
    def startup(self):

        print("[DEVICE] Starting up")
        self.server.send_event("EVENT", "ready")

    # Wait for a button to be pressed
    def wait_for_button(self):

        ev3 = self.hw.ev3
        # Wait until ANY button is pressed
        while not any(ev3.buttons.pressed()):
            wait(50)

        # Wait for button release to prevent double‑trigger
        while any(ev3.buttons.pressed()):
            wait(50)
            
    # Wait for gate to open
    def wait_for_gate(self):

        ev3 = self.hw.ev3
        ev3.speaker.say('Press any button')
        self.wait_for_button()

        ev3.speaker.say("[DEVICE] Waiting for start command")
        # Wait until distance is CLEAR
        while True:
            distance = self.hw.sensor_us.distance()
            if distance is not None and distance >= THRESHOLD_OBSTACLE:
                break
            wait(100)

    # Define main function
    def run(self):
        
        # Calibrate
        self.calibrate()
        # Wait for gate
        self.wait_for_gate()
        # Send event
        self.startup()

        cmd_raw = None
        last_cmd = None
        self.clock.reset()
    
        while True:
            # Send status            
            now = self.clock.time() / 1000  # s
            
            if now - self.last_heartbeat > HEARTBEAT_INTERVAL:
                self.heartbeat()
                self.last_heartbeat = now
            
            # Receive command
            cmd_raw = self.server.get_command()
            #cmd = cmd_raw.lower()

            if isinstance(cmd_raw, str):
                cmd = cmd_raw.lower()
            else:
                cmd = None

            if cmd:
                if cmd != last_cmd:
                    self.handle_command(cmd)
                    # Send event command consumed to Flask server 
                    self.server.send_event("CMD_ACK", cmd)
                    last_cmd = cmd                    
            else:
                last_cmd = None

            if self.start_received:
                self.periodic()

            wait(int(POLL_INTERVAL * 1000))

# Declare main program
if __name__ == "__main__":

    device = Device(
        DEVICE_NAME,
        SERVER
    )

    device.run()