#!/usr/bin/env pybricks-micropython

from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
                                 SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase

import ubinascii
import ujson
import urequests
import utime
import re

#Constant Variables: 
GRAB_SPEED = 1000
GRAB_CLOSED_ANGLE = 40
CHARACTER_DISPLAY_LIMIT = 20

RANDOM_FACT_URL = 'https://uselessfacts.jsph.pl/random.json?language=en'

#Assigning Ports to Motors and Sensors
left_wheel = Motor(Port.A, Direction.CLOCKWISE, [36, 12])
right_wheel = Motor(Port.D, Direction.CLOCKWISE, [36, 12])
grabber = Motor(Port.B, Direction.CLOCKWISE, [24, 24])
touch_sensor = TouchSensor(Port.S1)

#Initial Conditions: 
speed = 1000
is_grabbing = False

# Function for grabbing an object
def grab_object():
     # Get the global reference
     global is_grabbing
     # If it is grabbing, release, and if it is not grabbing, grab it
     grabber.run_target(GRAB_SPEED, (GRAB_CLOSED_ANGLE, 0)[is_grabbing], Stop.HOLD, False)
     is_grabbing = not is_grabbing 

#Function to setup the System Link Cloud in the code
def SL_setup():
     urlBase = "https://api.systemlinkcloud.com/nitag/v2/tags/"
     headers = {"Accept": "application/json", "x-ni-api-key": Key}
     return urlBase, headers

#Function to put up values in the System Link Cloud
def Put_SL(Tag, Type, Value):
     urlBase, headers = SL_setup()
     urlValue = urlBase + Tag + "/values/current"
     propValue = {"value": {"type": Type, "value": Value}}
     try:
          reply = urequests.put(urlValue, headers=headers, json=propValue).text
     except Exception as e:
          print(e)
          reply = 'failed'
     return reply

#Function to get the tag from the System Link Cloud
def Get_SL(Tag):
     urlBase, headers = SL_setup()
     urlValue = urlBase + Tag + "/values/current"
     try:
          value = urequests.get(urlValue, headers=headers).text
          data = ujson.loads(value)
          # print(data)
          result = data.get("value").get("value")
     except Exception as e:
          print(e)
          result = 'failed'
     return result

#Function to create a tag in the code
def Create_SL(Tag, Type):
     urlBase, headers = SL_setup()
     urlTag = urlBase + Tag
     propName = {"type": Type, "path": Tag}
     try:
          urequests.put(urlTag, headers=headers, json=propName).text
     except Exception as e:
          print(e)


# Function for displaying fact on EV3 screen
def display_random_fact():
     # Clear the display
     brick.display.clear()
     # Get the text of the random fact
     response = ' ' + ujson.loads(urequests.get(RANDOM_FACT_URL).text)['text'] + ' '
     last_space_index = 0
     last_line_break_index = 0
     # Loop through the response
     for i, char in enumerate(response):
          if char == ' ':
               last_space_index = i
          if i - last_line_break_index == CHARACTER_DISPLAY_LIMIT:
               # Print up to the most recent space to avoid cutting off words
               brick.display.text(response[last_line_break_index+1:last_space_index])
               last_line_break_index = last_space_index
     # Print the remainder of the text
     brick.display.text(response[last_line_break_index+1:])

# Beep when ready
brick.sound.beep()

iter = 0
previously_touching = False
while True:
     # Motion controls - GET related data
     try:
          # Get request
          command = int(Get_SL('Command'))
          if command == 0:                   # No command
               left_wheel.stop(Stop.COAST)
               right_wheel.stop(Stop.COAST)
          elif command == 1:                 # Grab
               grab_object()
          elif command == 2:                 # Move forward
               left_wheel.run(speed)
               right_wheel.run(speed)
          elif command == 3:                 # Move backward
               left_wheel.run(-speed)
               right_wheel.run(-speed)
          elif command == 4:                 # Turn left (CCW)
               left_wheel.run(speed / 2)
               right_wheel.run(-speed / 2)
          elif command == 5:                 # Turn right (CW)
               left_wheel.run(-speed / 2)
               right_wheel.run(speed / 2)
          elif command == 6:                 # Turn forward-left
               left_wheel.run(speed)
               right_wheel.run(speed / 2)
          elif command == 7:                 # Turn forward-right
               left_wheel.run(speed / 2)
               right_wheel.run(speed)
          elif command == 8:                 # Turn back-left
               left_wheel.run(-speed)
               right_wheel.run(-speed / 2)
          elif command == 9:                 # Turn back-right
               left_wheel.run(-speed / 2)
               right_wheel.run(-speed)
          elif command == 10:                # Slow down
               speed -= 25
               if speed < 25:                # Minimum speed is 25
                    speed = 25
          elif command == 11:                # Speed up
               speed += 25
          else:                              # Unknown command, so just stop
               print('Unknown command: ' + command)
               left_wheel.stop(Stop.COAST)
               right_wheel.stop(Stop.COAST)
     except Exception as e:
          print('Exception was thrown when trying to get \'Command\' tag value:\n' + e)

     # Post results from TouchSensor if something changes
     currently_touching = touch_sensor.pressed()
     if currently_touching != previously_touching:
          Put_SL('TouchSensor', 'BOOLEAN', str(currently_touching))
          previously_touching = currently_touching

     # For displaying useless info on screen
     if iter >= 10:
          display_random_fact()
          iter = 0
     else:
          iter += 1