# 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:
    # Print out current distance              To convert mm to cm, just divide by 10
    print("Distance from nearest object is {}cm".format((Sonar.getDistance() / 10)))
    # If the rover is less than 10cm from an object, then we need to avoid it.
    if Sonar.getDistance() < TRIGGER_DISTANCE:
        # Set Motor velocity to zero 
        LeftMotors.setTargetVelocity(MOTOR_STOP)
        RightMotors.setTargetVelocity(MOTOR_STOP)
        # Print out that an object is too close
        print("Obstacle in the way, moving around it.")
        # Reverse Direction
        LeftMotors.setTargetVelocity(MOTORS_REVERSE)
        RightMotors.setTargetVelocity(MOTORS_REVERSE)
        # Wait 2s to give time for reverse 
        time.sleep(2)
        # Turn left until distance is greater than 15cm 
        while Sonar.getDistance() < SAFE_DISTANCE:    
            # Start left turn by putting left wheels in reverse and right wheels forward
            LeftMotors.setTargetVelocity(MOTORS_REVERSE)
            RightMotors.setTargetVelocity(MOTORS_FORWARD)
            # Wait .01s for turn
            time.sleep(.01)
        # Set Motor velocity to zero 
        LeftMotors.setTargetVelocity(MOTOR_STOP)
        RightMotors.setTargetVelocity(MOTOR_STOP)
    # Start forward movement
    LeftMotors.setTargetVelocity(MOTORS_FORWARD)
    RightMotors.setTargetVelocity(MOTORS_FORWARD)