import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
import time

from RoboidControl.Participants.SiteServer import SiteServer
from RoboidControl.Participants.ParticipantUDP import ParticipantUDP
from RoboidControl.Thing import *
from RoboidControl.Things.DifferentialDrive import DifferentialDrive
from RoboidControl.Things.TouchSensor import TouchSensor

# Start a site server
site = SiteServer(port=7691)

# Create a local participant for handling communcation
# using default settings (UDP communciation over port 7681)
participant = ParticipantUDP(port=7691, local_port=7683, ip_address="127.0.0.1")
# The robot's propulsion is a differential drive
bb2b = DifferentialDrive(participant)
bb2b.name = "BB2B"
bb2b.SetDriveDimensions(0.064, 0.128)
bb2b.SetOrientation(SwingTwist.Degrees(40, 0, 0))
bb2b.SetPosition(Spherical(0.15, Direction.up))

model = Thing(parent=bb2b)
model.model_url = "https://passer.life/extras/Roller1anim1.png"
model.SetOrientation(SwingTwist.Degrees(90, 0, 0))

# It has a touch sensor at the front left of the roboid
touch_left = TouchSensor(parent=bb2b)
touch_left.name = "Touch left"
touch_left.SetPosition(Spherical(0.12, Direction.Degrees(-30, 0)))

# and other one on the right
touch_right = TouchSensor(parent=bb2b)
touch_right.name = "Touch right"
touch_right.SetPosition(Spherical(0.12, Direction.Degrees(30, 0)))

# Do forever:
while True:
    # The left wheel turns forward when nothing is touched on the right side
    # and turn backward when the roboid hits something on the right
    wheel_speed_left = -600 if touch_right.touched_something else 600
    # The right wheel does the same, but instead is controlled by
    # touches on the left side
    wheel_speed_right = -600 if touch_left.touched_something else 600
    # When both sides are touching something, both wheels will turn backward
    # and the roboid will move backwards
    bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right)
    if touch_left.touched_something or touch_right.touched_something:
        print(f'{wheel_speed_left} {wheel_speed_right} {bb2b.linear_velocity.distance} {bb2b.angular_velocity.distance}')

    # Update the roboid state
    participant.Update()
    # and sleep for 100ms
    time.sleep(0.1)