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

from ev3dev2.button import Button
from ev3dev2.display import Display
from ev3dev2.led import Leds
from ev3dev2.motor import LargeMotor, MediumMotor, MoveSteering, MoveTank, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, GyroSensor, InfraredSensor, TouchSensor, UltrasonicSensor
from ev3dev2.sound import Sound

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

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
        import requests
        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
        #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_up = MediumMotor(OUTPUT_A)
        self.motor_left = LargeMotor(OUTPUT_B)
        self.motor_right = LargeMotor(OUTPUT_C)
        self.motor_down = MediumMotor(OUTPUT_D)
        self.move_tank = MoveTank(OUTPUT_B, OUTPUT_C)
        self.move_steering = MoveSteering(OUTPUT_B, OUTPUT_C)
        # Initialize sensors
        self.sensor_colour = ColorSensor(INPUT_1)
        self.sensor_gyro = GyroSensor(INPUT_2)
        #self.sensor_ir = InfraredSensor(INPUT_4)
        self.sensor_touch = TouchSensor(INPUT_3)
        self.sensor_us = UltrasonicSensor(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

    # 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

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

            time.sleep(POLL_INTERVAL)

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

    device = Device(
        DEVICE_NAME,
        SERVER
    )

    device.run()