#!/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 time, requests, random

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

from pixycamev3.pixy2 import Pixy2

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

POLL_INTERVAL = 1.0
HEARTBEAT_INTERVAL = 2.0

#TARGET_SIG = 1     # Pixy2 signature to follow
TARGET_SIG = 2      # Pixy2 signature to follow
FRAME_CENTER = 158  # Pixy2 resolution of 316x208
KP = 0.4            # Steering gain

OBSTACLE_DISTANCE = 150  # mm

# -------------------
# 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 = 125 # mm distance between middle of tire contact
        self.wheel_diameter = 43.2 # mm
        # Initialize motors
        self.motor_arms = MediumMotor(OUTPUT_A)
        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(1, 0) # Turn on 1 of Pixy2 LEDs for less reflection
        self.pixy.set_lamp(0, 0) # Turn off Pixy2 LEDs for less reflection

    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
        self.waiting = False

    # Attack obstacle
    def attack_obstacle(self):

        distance_cm = self.hw.sensor_us.distance_centimeters
        
        if distance_cm is None:
            return False  # Treat as no obstacle
        
        distance = distance_cm * 10

        arms = self.hw.motor_arms
        tank = self.hw.move_tank
        
        if distance < OBSTACLE_DISTANCE:
            print("[OBS] Obstacle detected at", distance, "mm")
            msg = "Prey:" + str(distance) + "mm"
            self.server.send_event("STATUS", msg)
            self.server.send_event("EVENT", "01")
            # Charge forward
            tank.on_for_seconds(SpeedPercent(60), SpeedPercent(60), 0.6)
            # Arms claw
            arms.on_for_seconds(SpeedPercent(80), 0.2)
            arms.on_for_seconds(SpeedPercent(-80), 0.2)            
            return True
            
        return False

    # Follow signature
    def follow_signature(self):

        pixy = self.hw.pixy
        steer = self.hw.move_steering

        # Read Pixy blocks
        try:
            status, blocks_raw = pixy.get_blocks(1, 1)
        except OSError:
            print("[PIXY] I2C error. Retrying ...")
            time.sleep(0.05)
            return
        
        # Send JSON to Flask server
        self.send_pixy_json(blocks_raw)
        # Filter blocks
        blocks_for_following = []

        if blocks_raw:
            for b in blocks_raw:
                if b.sig == TARGET_SIG:
                    blocks_for_following.append(b)

        # Pick largest block
        block = self.get_largest_block(blocks_for_following)

        if block:
            # Compute steering error
            error = block.x_center - FRAME_CENTER
            turn = KP * error
            turn = max(min(turn, 100), -100)

            message = '[FOLLOW] x:' + str(block.x_center) + ', Turn:' + str(turn)
            print(message)
        
            # Drive forward while steering
            steer.on(-turn, SpeedPercent(20))

        else:
            print("[FOLLOW] Object not found")
            steer.off()
        
    # Find largest block
    def get_largest_block(self, blocks):

        if not blocks:
            return None
        return max(blocks, key=lambda b: b.width * b.height)
        
    # Send status 'alive' to Flask server
    def heartbeat(self):
        self.server.send_event("STATUS", "alive")

    # 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.waiting = False

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

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

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

    # Define functions run periodically
    def periodic(self):
        # MODIFY TO SUIT REQUIRED CODE
        # If waiting, growl periodically
        if self.waiting:
            value = random.randint(1, 5)
            if value == 1:
                self.server.send_event("EVENT", "01")
            
        else:
            # Attack takes priority
            if self.attack_obstacle():
                return
                
            # Otherwise follow signature
            self.follow_signature()
            time.sleep(0.02) # 20ms delay to prevent I2C overload

    # Send Pixy2 JSON to Flask server endpoint
    def send_pixy_json(self, blocks_raw):

        blocks_json = []

        if blocks_raw:
            for b in blocks_raw:
                print(vars(b))
                if b.sig == TARGET_SIG:
                    blocks_json.append({
                        "x": 316 - b.x_center,
                        "y": 208 - b.y_center,            
                        "w": b.width,
                        "h": b.height,
                        "sig": b.sig,
                        "angle": getattr(b, "angle", None)
                    })

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

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

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

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

    # Define main function
    def run(self):
        # Send event
        self.startup()
        last_cmd = None
                
        while True:
            # Send status
            now = time.time()
            if now - self.last_heartbeat > HEARTBEAT_INTERVAL:
                self.heartbeat()
                self.last_heartbeat = now
            # Receive command
            cmd = self.server.get_command()

            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

            #self.periodic()
            if self.start_received:
                self.periodic()

            time.sleep(POLL_INTERVAL)

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

    device = Device(
        DEVICE_NAME,
        SERVER
    )

    device.run()