using LinearAlgebra;

namespace RoboidControl {

    // The robot is based on a differential drive
    public class BB2B : DifferentialDrive {
        readonly DifferentialDrive drive;
        readonly TouchSensor touchLeft;
        readonly TouchSensor touchRight;
        const float speed = 0.5f;

        public BB2B(Participant owner) : base(owner) {
            this.name = "BB2B";
            this.SetMotors(new Motor(this), new Motor(this));
            this.SetDriveDimensions(0.064f, 0.128f);

            // Is has a touch sensor at the front left of the roboid
            touchLeft = new(this) {
                position = Spherical.Degrees(0.12f, -30, 0),
                orientation = new SwingTwist(-30, 0, 0)
            };
            touchRight = new(this) {
                position = Spherical.Degrees(0.12f, 30, 0),
                orientation = new SwingTwist(30, 0, 0)
            };
        }

        public override void Update(ulong currentTimeMs, bool recurse = 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 ? -speed : speed;

            // The right wheel does the same, but instead is controlled by
            // touches on the left side
            float rightWheelSpeed = touchLeft.touchedSomething ? -speed : speed;

            // When both sides are touching something, both wheels will turn backward
            // and the roboid will move backwards

            this.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);

            base.Update(currentTimeMs, recurse);
        }
    }

}