#!/usr/bin/env python3

# Import modules
import os
# Prevent requests from loading optional tools which slow down start
os.environ['PYTHONHTTPSVERIFY'] = '0'
os.environ['REQUESTS_CA_BUNDLE'] = ''
os.environ['SSL_CERT_FILE'] = ''

import math, time, requests

from ev3dev2.button import Button
from ev3dev2.motor import LargeMotor, MoveSteering, MoveTank, OUTPUT_B, OUTPUT_C, SpeedPercent
from ev3dev2.sensor import INPUT_4
from ev3dev2.sensor.lego import UltrasonicSensor

from pixycamev3.pixy2 import Pixy2, MainFeatures

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

# Mapping of barcode ID to barcode ID code in orchestration
BC_MAP = {
    "1": "01",
    "2": "02",
    "3": "03",
    "4": "04",
    "5": "05",
    "6": "06",
    "7": "01",
    "8": "02",
    "9": "03",
    "10": "04",
    "11": "05",
    "12": "06"
}

POLL_INTERVAL = 1.0
HEARTBEAT_INTERVAL = 2.0

FAST_LOOP = 0.02 # 20 ms
SLOW_LOOP = 0.5  # 500 ms

X_REF = 39     # Center of Pixy2’s field of view
#SCALE = 8      # Make line big enough
#OFFSET_X = 0   # Adjust to centre horizontally
#OFFSET_Y = 150 # Push it down

#KP = 1.2 # Steering gain
#KP = 0.8 # Steering gain
#KP = 0.6 # Steering gain
#BASIC_SPEED = 10
#GAIN = 0.4

OBSTACLE_DISTANCE = 150  # mm
OBSTACLE_TIMEOUT = 3.0 # s

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

        print("[SERVERCLIENT] Initializing")
        self.server = server
        self.device = device
        self.requests = requests
        
    # 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 = self.requests.post(self.server + "/event", json=payload, timeout=POLL_INTERVAL, headers={"Connection": "close"} )
            r.close()
            print("[EVENT]", payload)

        except Exception as e:
            print("[HTTP] Sending event failed with error '", e, "'")
            time.sleep(0.2)
    
    # Receive command
    def get_command(self):
        # Try receiving JSON command from /sent_command/ endpoint on Flask server
        try:
            r = self.requests.get(self.server + "/sent_command/" + self.device, timeout=POLL_INTERVAL, 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, "'")
            time.sleep(0.2)
            return None

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

        print("[HARDWARE] Initializing")
        # MODIFY TO SUIT REQUIRED HARDWARE
        # Initialize robot constants
        self.axle_track = 135 # mm distance between middle of tire contact
        self.wheel_diameter = 45 # mm
        # Initialize motors
        self.motor_left = LargeMotor(OUTPUT_B)
        self.motor_right = LargeMotor(OUTPUT_C)
        self.move_tank = MoveTank(OUTPUT_B, OUTPUT_C)
        self.move_steering = MoveSteering(OUTPUT_B, OUTPUT_C)
        # Initialize sensors
        self.pixy = Pixy2(port=1, i2c_address=0x54)
        self.sensor_us = UltrasonicSensor(INPUT_4)
        self.pixy.set_lamp(0, 0) # Turn off Pixy2 LEDs for less reflection
        self.button = Button()

        self.last_uid = None

    def forward(self):
        print("[HW] Running command forward")

    def backward(self):
        print("[HW] Running command backward")

    def start(self):
        print("[HW] Running command start")

    def stop(self):
        print("[HW] Running command stop")

    def beep(self):
        print("[HW] Running command 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.last_heartbeat = 0
        self.start_received = False
        # Obstacle handling
        self.obstacle_start_time = None
        self.avoiding = False
        self.data = MainFeatures()
        self._BASIC_SPEED = 12
        self._GAIN = 0.35
        self.waiting = False

    # Find barcodes
    def check_bcid(self, data):
        # Send JSON for dashboard
        if not data or not data.barcodes:
            self.hw.last_uid = None
            return

        # Take first barcode
        bc = data.barcodes[0]
        code_str = str(bc.code)
        
        if code_str == self.hw.last_uid:
            return  # Already processed

        # Map to orchestration
        self.hw.last_uid = code_str
        mapped = BC_MAP.get(code_str)
        
        if mapped:
            msg = self.name + ":BC:" + mapped
            print("[BARCODE] UID:", code_str, "Mapped:", mapped)
            self.server.send_event("BARCODE", mapped)
        else:
            print("[BARCODE] Unknown barcode", code_str)

    # Run fast loop 
    def fast_tick(self):
        # Handle obstacle 
        if self.handle_obstacle():
            return
        
        # Line follow
        try:
            data = self.hw.pixy.get_linetracking_data()
        except OSError:
            print("[PIXY] I2C error. Retrying ...")
            return

        self.follow_line(data)

        data.clear()

    # Follow line
    def follow_line(self, data):
        # MODIFY TO SUIT REQUIRED CODE
        if not data or not data.vectors:
            print("[LINE] Line not found")
            self.hw.move_tank.stop()
            return
        
        # If vector found, calculate horizontal distance from middle
        if data.number_of_vectors > 0:
            dx = data.vectors[0].x1 - X_REF
            self.move(dx)
    
    # 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":
            #self.hw.start()
            # If command is 'start', allow code to proceed to periodic()                    
            self.start_received = True
            self.server.send_event("EVENT", "08")

        elif cmd == "wait":
            self.waiting = True
            self.hw.stop()
            self.wait_for_obstacle()

        elif cmd == "continue":
            self.waiting = False
            # If command is 'continue', allow code to proceed to periodic()                    
            self.start_received = True

        elif cmd == "stop":
            #self.hw.stop()
            self.hw.move_tank.stop()
            self.server.send_event("EVENT", "03")
            self.server.send_event("EVENT", "04")
            self.server.send_event("EVENT", "05")
            self.server.send_event("EVENT", "06")
            time.sleep(3.0)
            self.server.send_event("EVENT", "07")

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

        else:
            print("[CMD] Receiving unknown command '", cmd, "'")

    # Detect obstacle
    def handle_obstacle(self):

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

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

        distance_mm = distance_cm * 10
        if distance_mm < OBSTACLE_DISTANCE:
            msg = "Obstacle:" + str(distance_mm) + "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, send stop command to Flask
            if not self.avoiding and (now - self.obstacle_start_time) > OBSTACLE_TIMEOUT:
                print("[OBS] Timeout. Attempting to drive around ...")
                self.avoiding = True
                self.server.send_event("COMMAND", "stop")
                self.obstacle_start_time = None
                self.avoiding = False
                
            return True
        else:
            self.obstacle_start_time = None
            return False
        
        # if distance_mm < OBSTACLE_DISTANCE:
        #     print("[OBS] Obstacle detected. Stopping ...")
        #     self.hw.move_tank.on(0, 0)
        #     return True

        # return False

    # Send status 'alive' to Flask server
    def heartbeat(self):
        self.server.send_event("STATUS", "alive")
    
    # Limit speed in range [-100, 100]
    def limit_speed(self, speed):

        if speed > 100:
            speed = 100
        elif speed < -100:
            speed = -100
        return speed
    
    # Move according to offset from centre
    def move(self, speed_x):

        speed_x *= self._GAIN
        speed_B = self.limit_speed(self._BASIC_SPEED + speed_x) 
        speed_C = self.limit_speed(self._BASIC_SPEED - speed_x) 
        
        self.hw.move_tank.on(speed_B, speed_C)
        
    # Send Pixy2 JSON to Flask server endpoint
    def send_pixy_json(self, data):

        vectors_json = []
        barcodes_json = []

        if data is not None:
            # If vectors found
            if data.vectors:
                for v in data.vectors:
                    # Compute angle manually for Pixy2
                    dx = v.x1 - v.x0
                    dy = v.y1 - v.y0
                    angle = math.degrees(math.atan2(dy, dx))
                    # Compute vector length
                    #length = math.sqrt(dx*dx + dy*dy)
                    #dx /= length
                    #dy /= length
                    # Extend vector length
                    #x0_ext = v.x0 * SCALE + OFFSET_X
                    #y0_ext = v.y0 * SCALE + OFFSET_Y
                    #x1_ext = v.x1 * SCALE + OFFSET_X
                    #y1_ext = v.y1 * SCALE + OFFSET_Y

                    vectors_json.append({
                        #"x0": x0_ext,
                        #"y0": y0_ext,
                        #"x1": x1_ext,
                        #"y1": y1_ext,
                        "x0": v.x0,
                        "y0": v.y0,
                        "x1": v.x1,
                        "y1": v.y1,
                        "angle": round(angle, 2)
                    })

            # If barcodes found
            if data.barcodes:
                for bc in data.barcodes:
                    barcodes_json.append({
                        "x": bc.x,
                        "y": bc.y,
                        "code": bc.code
                    })

        payload = {
            "device": self.name,
            "blocks": [],
            "vectors": vectors_json,
            "barcodes": barcodes_json
        }

        try:
            requests.post(self.server.server + "/receive_frame", json=payload, timeout=0.2)
        except:
            pass        

    # Run slow loop for Flask interaction
    def slow_tick(self, last_cmd):
        # Heartbeat
        self.heartbeat()

        # Receive command
        cmd = self.server.get_command()

        if cmd and 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

        # Pixy snapshot for dashboard & barcodes
        try:
            data = self.hw.pixy.get_linetracking_data()
        except OSError:
            print("[PIXY] I2C error. Slow loop, skipping frame ...")
            return last_cmd

        # Send JSON for dashboard
        self.send_pixy_json(data)

        # Find barcodes
        self.check_bcid(data)

        data.clear()

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

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

    # Wait for Indie to pass in front
    def wait_for_obstacle(self):

        distance_cm = self.hw.sensor_us.distance_centimeters

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

        distance_mm = distance_cm * 10

        while distance_mm > OBSTACLE_DISTANCE:

            distance_cm = self.hw.sensor_us.distance_centimeters

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

            distance_mm = distance_cm * 10
        
        while distance_mm < OBSTACLE_DISTANCE:

            distance_cm = self.hw.sensor_us.distance_centimeters

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

            distance_mm = distance_cm * 10

        self.server.send_event("EVENT", "09")

    # Define main function
    def run(self):
        # Send event
        self.startup()
        last_cmd = None
        last_fast = time.time()
        last_slow = time.time()
                
        while True:

            # Emergency stop
            if self.hw.button.enter:
                print("[EMERGENCY] STOP button pressed")
                self.hw.move_tank.stop()
                raise SystemExit
            
            now = time.time()
            # 2 loops to allow line following to happen 'all' the time and Flask interaction less often
            # -------------------------
            # FAST LOOP - Obstacle detection & line following
            # -------------------------
            if self.start_received:
                if now - last_fast >= FAST_LOOP:
                    self.fast_tick()
                    last_fast = now

            # -------------------------
            # SLOW LOOP - Flask interaction
            # -------------------------
            if now - last_slow >= SLOW_LOOP:
                last_cmd = self.slow_tick(last_cmd)
                last_slow = now

            time.sleep(0.005) # 5 ms idle

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

    device = Device(
        DEVICE_NAME,
        SERVER
    )

    device.run()