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;

        public BB2B(Participant owner) : base(owner) {
            this.name = "BB2B";
            this.wheelRadius = 0.032f;
            this.wheelSeparation = 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 ? -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

            this.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);

            base.Update(currentTimeMs, recurse);
        }
    }

}