#!/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

from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor import INPUT_4
from ev3dev2.sensor.lego import InfraredSensor

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

# Initialize program constants
SPEED_LIFT = 750
# Motor speeds
SPEED_HIGH = 100
SPEED_LOW = 50
SPEED_MEDIUM = 75
# Motor durations
DURATION_HIGH = 4.0
DURATION_LOW = 1.0
DURATION_MEDIUM = 2.0
# Wait durations
WAIT_LIFT = 10
WAIT_WHILE = 0.1

POLL_INTERVAL = 1.0
HEARTBEAT_INTERVAL = 2.0

# -------------------
# 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 = 110 # mm distance between middle of tire contact
        self.wheel_diameter = 43.2 # mm
        # Initialize motors
        self.motor_move = LargeMotor(OUTPUT_A)
        self.motor_turn = MediumMotor(OUTPUT_B)
        self.motor_eat = LargeMotor(OUTPUT_C)        
        # Mindsensors motor multiplexer on port INPUT_1 using I2C at address x03
        # Large motors need to be defined even though medium motor used
        self.motor_lift_right = LargeMotor('in1:i2c3:M1') 
        self.motor_lift_left = LargeMotor('in1:i2c3:M2')
        # Initialize sensors
        self.sensor_ir = InfraredSensor(INPUT_4)

    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.leaves_not_detected = True

    # Detect leaves
    def detect_leaves(self):

        ir = self.hw.sensor_ir

        # heading: -25..25, distance: 0..100 (cm)
        heading, distance = ir.heading_and_distance(channel=1)

        message = 'Heading: ' + str(heading) + ' Distance: ' + str(distance)
        print(message)
    
        # Turn toward beacon (softened)
        self.turn(heading)

        # No distance reading, do nothing
        if distance is None or distance == 0:
            return False

        # Soft approach zone (40–30 cm)
        if 30 < distance <= 40:
            print("[LEAVES] Soft approach ...")
            self.hw.motor_move.on_for_seconds(SPEED_LOW, 0.3)
            return False

        # Eating zone (< 30 cm)
        if distance <= 30:
            print("[LEAVES] Leaves detected. Stopping ...")
            self.hw.motor_move.stop()
            self.lift_dino()
            return True

        return False

    # Eat leaves
    def eat_leaves(self):

        jaw = self.hw.motor_eat

        print("[EAT] Eating leaves ...")

        # Open jaw
        jaw.on_for_seconds(SPEED_LOW, DURATION_HIGH)
        # Pause
        time.sleep(3)
        # Close jaw
        jaw.on_for_seconds(-SPEED_LOW, DURATION_HIGH)
        print("[EAT] Done eating leaves ...")
        self.server.send_event("EVENT", "01")
        
        self.leaves_not_detected = False
        return self.leaves_not_detected
        
    # 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

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

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

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

    # Lift dinosaur
    def lift_dino(self):

        left = self.hw.motor_lift_left
        right = self.hw.motor_lift_right
        move = self.hw.motor_move
        turn = self.hw.motor_turn

        print("[LIFT] Lifting Brontie ...")
        msg = "Standing on hind legs"
        self.server.send_event("STATUS", msg)

        # Lift up
        left.run_forever(speed_sp=-SPEED_LIFT)
        right.run_forever(speed_sp=-SPEED_LIFT)
        time.sleep(WAIT_LIFT)

        left.stop()
        right.stop()
        # Eat leaves while lifted
        self.eat_leaves()
        # Lower down
        left.run_forever(speed_sp=SPEED_LIFT)
        right.run_forever(speed_sp=SPEED_LIFT)
        time.sleep(WAIT_LIFT)

        left.stop()
        right.stop()
        move.stop()
        turn.stop()

        print("[LIFT] Done lifting Brontie ...")
        
    # Define functions run periodically
    def periodic(self):
        # MODIFY TO SUIT REQUIRED CODE
        # Try to detect leaves
        if self.leaves_not_detected:
            self.detect_leaves()
        else:
            return

        # Otherwise wander in snake pattern
        move = self.hw.motor_move
        turn = self.hw.motor_turn

        # Wiggle left & right
        turn.on_for_seconds(SPEED_LOW, 0.3)
        move.on_for_seconds(SPEED_LOW, 0.3)
        turn.on_for_seconds(-SPEED_LOW, 0.3)
        move.on_for_seconds(SPEED_LOW, 0.3)

    # Shake body
    def shake_body(self):

        eat = self.hw.motor_eat
        move = self.hw.motor_move
        turn = self.hw.motor_turn

        print("[SHAKE] Brontie shaking ...")

        eat.on_for_seconds(-SPEED_LOW, 0.2)
        move.on_for_seconds(SPEED_LOW, 0.2)

        turn.on_for_seconds(SPEED_MEDIUM, 0.2)
        turn.on_for_seconds(-SPEED_MEDIUM, 0.3)
        turn.on_for_seconds(SPEED_MEDIUM, 0.2)
        
    # Send event 'ready' to Flask server
    def startup(self):

        print("[DEVICE] Starting up")
        self.server.send_event("EVENT", "ready")
        
    # Turn according to IR sensor heading
    def turn(self, heading):
        # Heading is -25 to 25 (0, 12 noon    12, 1 pm    25, 2 pm    -12, 11 am  -25, 10 am)
        move = self.hw.motor_move
        turn = self.hw.motor_turn

        # Forward 12 noon
        if -5 < heading <= 5:
            print("[HEADING] Forward")
            move.on_for_seconds(SPEED_LOW, DURATION_LOW)
            msg = "Heading: Forward"

        # Right 1 pm, turn slightly right
        elif 5 < heading <= 15:
            print("[HEADING] Right 1 pm")
            turn.on_for_seconds(SPEED_LOW, DURATION_LOW)
            move.on_for_seconds(SPEED_LOW, DURATION_LOW)
            msg = "Heading: Right 1 pm"

        # Right 2 pm, turn more right
        elif 15 < heading <= 25:
            print("[HEADING] Right 2 pm")
            turn.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
            move.on_for_seconds(SPEED_LOW, DURATION_LOW)
            msg = "Heading: Right 2 pm"

        # Left 11 am, turn slightly left
        elif -15 < heading <= -5:
            print("[HEADING] Left 11 am")
            turn.on_for_seconds(-SPEED_LOW, DURATION_LOW)
            move.on_for_seconds(SPEED_LOW, DURATION_LOW)
            msg = "Heading: Left 11 am"

        # Left 10 am, turn more left
        elif -25 < heading <= -15:
            print("[HEADING] Left 10 am")
            turn.on_for_seconds(-SPEED_MEDIUM, DURATION_LOW)
            move.on_for_seconds(SPEED_LOW, DURATION_LOW)
            msg = "Heading: Left 10 am"

        else:
            print("[HEADING] Out of range")
            msg = "Heading: Out of range"

        self.server.send_event("STATUS", msg)

    # Define main function
    def run(self):
        # Send event
        self.startup()
        last_cmd = None
        # Shake body
        self.shake_body()
                
        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()