# Written For Python3+
# Written By: InformalAbsence
# Email: informalabsence@gmail.com

# Import required Phidgets Libraries
from Phidget22.Devices.DCMotor import *
from Phidget22.Devices.DistanceSensor import *
from Phidget22.Net import *

# Import time for use in sleep statement
import time 

############## Constants ##############
"""
You may need to change these values depending on how your rover moves.
See Step 3 for instructions on what values it should have.
"""

# Motor Speed
MOTOR_SPEED = 0.5
# Trigger Distance (At what distance should the rover stop in mm)
TRIGGER_DISTANCE = 100
# Safe Distance (At what distance should the rover start moving again in mm)
SAFE_DISTANCE = 150

# Dont Change Any of these for now 
# Motors Forward
MOTORS_FORWARD = MOTOR_SPEED
# Motors Reverse
MOTORS_REVERSE = (MOTOR_SPEED * -1)
# Motor Stop, this will always be the same 
MOTOR_STOP = 0.25

########### End of Constants ###########

# Connect to the wireless rover at ip 192.168.100.1 and port 5661
Net.addServer("", "192.168.100.1", 5661, "", 0)

# Initialize Motor Objects 
LeftMotors = DCMotor()
RightMotors = DCMotor()
# Initialize Sonar
Sonar = DistanceSensor()

# Address motor channels 
LeftMotors.setChannel(0)
RightMotors.setChannel(1)

# Open Objects with 5000ms delay
LeftMotors.openWaitForAttachment(5000)
RightMotors.openWaitForAttachment(5000)
Sonar.openWaitForAttachment(5000)

# Start self driving loop
while True:
    # Check if rover is too close to object
    # If rover is too close then move around it 
    # Else Start forward movement
    
    pass