using LinearAlgebra;

namespace RoboidControl {

    // The robot is based on a differential drive
    public class BB2B_Encoder : DifferentialDrive {
        readonly DifferentialDrive drive;
        readonly TouchSensor touchLeft;
        readonly TouchSensor touchRight;
        const float speed = 180.0f; // wheel rotation speed in degrees

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

            // Update the basic motors to motors with encoder
            EncoderMotor leftMotor = new(this, new RelativeEncoder()) {
                position = new Spherical(0.064f, Direction.left)
            };
            EncoderMotor rightMotor = new(this, new RelativeEncoder()) {
                position = new Spherical(0.064f, Direction.right)
            };
            this.SetMotors(leftMotor, rightMotor);

            // 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 leftWheelVelocity = touchRight.touchedSomething ? -speed : speed;

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

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

            this.SetWheelAngularVelocity(leftWheelVelocity, rightWheelVelocity);

            base.Update(currentTimeMs, recurse);
        }
    }

}