#!/usr/bin/env pybricks-micropython

# Import modules
import math, sys, time, urandom
from ucollections import namedtuple
import urequests as requests

from pybricks.ev3devices import ColorSensor, GyroSensor, InfraredSensor, Motor, TouchSensor, UltrasonicSensor
from pybricks.hubs import EV3Brick
from pybricks.parameters import Button, Color, Direction, Port, Stop
from pybricks.media.ev3dev import Font, ImageFile, SoundFile
from pybricks.robotics import DriveBase
from pybricks.tools import DataLog, StopWatch, wait 

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

POLL_INTERVAL = 1.0 # ms
HEARTBEAT_INTERVAL = 2.0 # s

# -------------------
# 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 = 105 # mm distance between middle of tire contact
        self.wheel_diameter = 55.5 # mm
        # Initialize motors
        self.motor_right = Motor(Port.A)
        self.motor_left = Motor(Port.B)
        #self.motor_right = Motor(Port.C)
        #self.motor_down = Motor(Port.D)
        self.drive_base = DriveBase(self.motor_left, self.motor_right, self.wheel_diameter, self.axle_track)
        # Initialize sensors
        #self.sensor_colour = ColorSensor(Port.S1)
        #self.sensor_gyro = GyroSensor(Port.S2)
        #self.sensor_ir = InfraredSensor(Port.S4)
        #self.sensor_touch = TouchSensor(Port.S3)
        self.sensor_us = UltrasonicSensor(Port.S1)

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

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

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

    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

    # Send event 'ready' to Flask server
    def startup(self):
        print("[DEVICE] Starting up")
        self.server.send_event("EVENT", "ready")

    # 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()
            
        elif cmd == "stop":
            self.hw.stop()

        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
        pass

    # 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()

            wait(int(POLL_INTERVAL * 1000))

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

    device = Device(
        DEVICE_NAME,
        SERVER
    )

    device.run()