package org.firstinspires.ftc.teamcode;

import com.vingvingrobot.follower.Follower;
import com.vingvingrobot.telemetry.SelectableOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

@TeleOp(name = "Tuning", group = "VingVingRobot")
public class Tuning extends SelectableOpMode {
    public static Follower follower;

    public Tuning() {
        super("Select a Tuning OpMode", s -> {
            s.folder("Localization", l -> {
                l.add("Localization Test", LocalizationTest::new);
                l.add("Forward Tuner", ForwardTuner::new);
                l.add("Lateral Tuner", LateralTuner::new);
                l.add("Turn Tuner", TurnTuner::new);
            });
            s.folder("Automatic", a -> {
                a.add("Forward Velocity Tuner", ForwardVelocityTuner::new);
                a.add("Lateral Velocity Tuner", LateralVelocityTuner::new);
                a.add("Forward Zero Power Acceleration", ForwardZPATuner::new);
                a.add("Lateral Zero Power Acceleration", LateralZPATuner::new);
            });
            s.folder("Manual", p -> {
                p.add("Translational Tuner", TranslationalTuner::new);
                p.add("Heading Tuner", HeadingTuner::new);
                p.add("Drive Tuner", DriveTuner::new);
                p.add("Centripetal Tuner", CentripetalTuner::new);
            });
            s.folder("Tests", t -> {
                t.add("Line", Line::new);
                t.add("Triangle", Triangle::new);
                t.add("Circle", Circle::new);
            });
        });
    }

    @Override
    public void onSelect() {
        if (follower == null) {
            follower = RobotConfig.createFollower(hardwareMap);
        }
        follower.setStartingPose(new Pose());
        Drawing.init();
    }
}

// Add individual tuning OpModes below...
// See the full example in Quickstart-master/TeamCode