using System.Threading;
using RoboidControl;

class BB2B {
    static void Main() {
        // The robot's propulsion is a differential drive
        DifferentialDrive bb2b = new();
        // It has a touch sensor at the front left of the roboid
        TouchSensor touchLeft = new(bb2b);
        // and other one on the right
        TouchSensor touchRight = new(bb2b);

        // 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
            float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
            // The right wheel does the same, but instead is controlled by
            // touches on the left side
            float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
            // When both sides are touching something, both wheels will turn backward
            // and the roboid will move backwards
            bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);

            // Update the roboid state
            bb2b.Update(true);
            // and sleep for 100ms
            Thread.Sleep(100);
        }
    }
}