public class BlueSkystoneSplines extends LinearOpMode {
    FtcDashboard dashboard = FtcDashboard.getInstance();
    int heightToPivot = 10;
    Telemetry dashboardTelemetry = dashboard.getTelemetry();
    Intake intake = new Intake();
    Lift lift = new Lift();
    Scorer scorer = new Scorer();
    Foundation foundation = new Foundation();
    int[] vals;
    SkystoneDetector detector = new SkystoneDetector();

    public void setup() {

    }

    public void runOpMode() throws InterruptedException {

        MecanumBase drive = new MecanumAuto(hardwareMap);
        DriveConstraints slowConstraints = new DriveConstraints(25.0, 20.0, 0.0,
                Math.toRadians(180.0), Math.toRadians(180.0), 0.0);
        DriveConstraints fastConstraints = new DriveConstraints(40.0, 20.0, 0.0,
                Math.toRadians(180.0), Math.toRadians(180.0), 0.0);
        intake.init(hardwareMap);
        lift.init(hardwareMap);
        scorer.init(hardwareMap);
        foundation.init(hardwareMap);


        Trajectory mid1 = new TrajectoryBuilder( new Pose2d(-33.0,63.0,Math.toRadians(180.0)),
                fastConstraints)
                .lineTo(new Vector2d(-22.0,42.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-150.0)))
                .addMarker(new Vector2d(-22.0, 42.0), ()->{
                    intake.collect(0.8);
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory mid2 = new TrajectoryBuilder(/*new Pose2d(-22.0, 42.0 ,Math.toRadians(180.0))*/mid1.end(),
        slowConstraints)
                .lineTo(new Vector2d(-32.0, 24.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-150.0)))
                .addMarker(new Vector2d(-32.0, 24.0), ()->{
                    intake.collect(0);
                    intake.hitter.setPosition(0.4);//in
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory mid3 = new TrajectoryBuilder(/*new Pose2d(-32.0, 24.0), Math.toRadians(180.0))*/mid2.end(),
                fastConstraints)
                .lineTo(new Vector2d(-22.0, 42.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-180.0)))
                .addMarker(new Vector2d(-22.0, 42.0), ()->{
                    scorer.gripper.setPosition(0);//down
                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(48.0, 33.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0, 33.0), ()->{
                    foundation.latchL.setPosition(1);//set down
                    foundation.latchR.setPosition(1);//set down

                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(1);
                    lift.Arm(-0.5,true);

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory mid4 = new TrajectoryBuilder(/*new Pose2d(48.0, 33.0, Math.toRadians(180.0))*/mid3.end(), slowConstraints)
                .splineTo(new Pose2d(48.0,42.0), new LinearInterpolator(Math.toRadians(90.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0,42.0), ()->{
                    scorer.gripper.setPosition(1);//open
                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(0.05);//closed
                    lift.Arm(-0.5, true);

                    foundation.latchL.setPosition(0);//up
                    foundation.latchR.setPosition(0);//up

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory mid5 = new TrajectoryBuilder(/*new Pose2d(48.0, 42.0, Math.toRadians(180.0))*/mid4.end(), fastConstraints)
                .lineTo(new Vector2d(-48.0, 42.0), new SplineInterpolator(Math.toRadians(-180.0), Math.toRadians(-150.0)))
                .addMarker(new Vector2d(-48.0, 42.0), ()->{
                    intake.collect(0.8);
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory mid6 = new TrajectoryBuilder(/*new Pose2d(-48.0, 42.0, Math.toRadians(180.0))*/mid5.end(), slowConstraints)
                .lineTo(new Vector2d(-56.0, 42.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(150.0)))
                .addMarker(new Vector2d(-56.0, 42.0), ()->{
                    intake.collect(0);
                    intake.hitter.setPosition(0.4);//hitter in

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory mid7 = new TrajectoryBuilder(/*new Pose2d(-56.0, 42.0, Math.toRadians(180.0))*/mid6.end(), fastConstraints)
                .lineTo(new Vector2d(-48.0, 42.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-180.0)))
                .addMarker(new Vector2d(-48.0, 42.0), ()-> {
                    scorer.gripper.setPosition(0);//down
                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(48.0, 42.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .addMarker(new Vector2d(48.0, 42.0), ()->{

                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(1);
                    lift.Arm(-0.5,true);

                    scorer.gripper.setPosition(1);//open
                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(0.05);//closed
                    lift.Arm(-0.5, true);

                    return Unit.INSTANCE;
                })

                .lineTo(new Vector2d(0.0, 42.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .build();

        //left stone
        Trajectory left1 = new TrajectoryBuilder( new Pose2d(-33.0,63.0,Math.toRadians(180.0)),
                fastConstraints)
                .lineTo(new Vector2d(-40.0,42.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-140.0)))
                .addMarker(new Vector2d(-40.0, 42.0), ()->{
                    intake.collect(0.8);
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory left2 = new TrajectoryBuilder(/*new Pose2d(-40.0, 42.0, Math.toRadians(180.0))*/left1.end(),
                slowConstraints)
                .lineTo(new Vector2d(-48.0, 24.0), new SplineInterpolator(Math.toRadians(-140.0), Math.toRadians(-140.0)))
                .addMarker(new Vector2d(-48.0, 24.0), ()->{
                    intake.collect(0);
                    intake.hitter.setPosition(0.4);//in
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory left3 = new TrajectoryBuilder(/*new Pose2d(-48.0, 24.0, Math.toRadians(180.0))*/ left2.end(),
                fastConstraints)
                .lineTo(new Vector2d(-22.0, 42.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-180.0)))
                .addMarker(new Vector2d(-22.0, 42.0), ()->{
                    scorer.gripper.setPosition(0);//down
                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(48.0, 33.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0, 33.0), ()->{
                    foundation.latchL.setPosition(1);//set down
                    foundation.latchR.setPosition(1);//set down

                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(1);
                    lift.Arm(-0.5,true);

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory left4 = new TrajectoryBuilder(/*new Pose2d(48.0, 33.0, Math.toRadians(180.0))*/left3.end(), slowConstraints)
                .splineTo(new Pose2d(48.0,42.0), new LinearInterpolator(Math.toRadians(90.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0,42.0), ()->{
                    scorer.gripper.setPosition(1);//open
                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(0.05);//closed
                    lift.Arm(-0.5, true);

                    foundation.latchL.setPosition(0);//up
                    foundation.latchR.setPosition(0);//up

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory left5 = new TrajectoryBuilder(/*new Pose2d(48.0, 42.0, Math.toRadians(180.0))*/left4.end(), fastConstraints)
                .lineTo(new Vector2d(-15.0, 42.0), new SplineInterpolator(Math.toRadians(-180.0), Math.toRadians(-135.0)))
                .addMarker(new Vector2d(-15.0, 42.0), ()->{
                    intake.collect(0.8);
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory left6 = new TrajectoryBuilder(/*new Pose2d(-15.0, 42.0, Math.toRadians(180.0))*/left5.end(), slowConstraints)
                .lineTo(new Vector2d(-23.0, 24.0), new SplineInterpolator(Math.toRadians(-135.0), Math.toRadians(-135.0)))
                .addMarker(new Vector2d(-23.0, 24.0), ()->{
                    intake.collect(0);
                    intake.hitter.setPosition(0.4);//hitter in

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory left7 = new TrajectoryBuilder(/*new Pose2d(-23.0, 24.0, Math.toRadians(180.0))*/left6.end(), fastConstraints)
                .lineTo(new Vector2d(-15.0, 42.0), new SplineInterpolator(Math.toRadians(-135.0), Math.toRadians(-180.0)))
                .addMarker(new Vector2d(-15.0, 42.0), ()-> {
                    scorer.gripper.setPosition(0);//down
                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(48.0, 42.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .addMarker(new Vector2d(48.0, 42.0), ()->{

                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(1);
                    lift.Arm(-0.5,true);

                    scorer.gripper.setPosition(1);//open
                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(0.05);//closed
                    lift.Arm(-0.5, true);

                    return Unit.INSTANCE;
                })

                .lineTo(new Vector2d(0.0, 42.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .build();
        Trajectory right1 = new TrajectoryBuilder( new Pose2d(-33.0,63.0, Math.toRadians(180.0)),
                fastConstraints)
                .lineTo(new Vector2d(-30.0,42.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-140.0)))
                .addMarker(new Vector2d(-30.0, 42.0), ()->{
                    intake.collect(0.8);
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory right2 = new TrajectoryBuilder(/*new Pose2d(-30.0, 42.0, Math.toRadians(180.0))*/ right1.end(),
                slowConstraints)
                .lineTo(new Vector2d(-38.0, 24.0), new SplineInterpolator(Math.toRadians(-140.0), Math.toRadians(-140.0)))
                .addMarker(new Vector2d(-38.0, 24.0), ()->{
                    intake.collect(0);
                    intake.hitter.setPosition(0.4);//in
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory right3 = new TrajectoryBuilder(/*new Pose2d(-38.0, 24.0, Math.toRadians(180.0))*/ right2.end(),
                fastConstraints)
                .lineTo(new Vector2d(-30.0, 42.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-180.0)))
                .addMarker(new Vector2d(-30.0, 42.0), ()->{
                    scorer.gripper.setPosition(0);//down
                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(48.0, 33.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0, 33.0), ()->{
                    foundation.latchL.setPosition(1);//set down
                    foundation.latchR.setPosition(1);//set down

                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(1);
                    lift.Arm(-0.5,true);

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory right4 = new TrajectoryBuilder(/*new Pose2d(48.0, 33.0, Math.toRadians(180.0))*/right3.end(), slowConstraints)
                .splineTo(new Pose2d(48.0,42.0), new LinearInterpolator(Math.toRadians(90.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0,42.0), ()->{
                    scorer.gripper.setPosition(1);//open
                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(0.05);//closed
                    lift.Arm(-0.5, true);

                    foundation.latchL.setPosition(0);//up
                    foundation.latchR.setPosition(0);//up

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory right5 = new TrajectoryBuilder(/*new Pose2d(48.0, 42.0, Math.toRadians(180.0))*/right4.end(), fastConstraints)
                .lineTo(new Vector2d(-53.0, 42.0), new SplineInterpolator(Math.toRadians(-180.0), Math.toRadians(-135.0)))
                .addMarker(new Vector2d(-53.0, 42.0), ()->{
                    intake.collect(0.8);
                    return Unit.INSTANCE;
                })
                .build();
        Trajectory right6 = new TrajectoryBuilder(/*new Pose2d(-53.0, 42.0, Math.toRadians(180.0))*/right5.end(), slowConstraints)
                .lineTo(new Vector2d(-61.0, 24.0), new SplineInterpolator(Math.toRadians(-135.0), Math.toRadians(-135.0)))
                .addMarker(new Vector2d(-61.0, 24.0), ()->{
                    intake.collect(0);
                    intake.hitter.setPosition(0.4);//hitter in

                    return Unit.INSTANCE;
                })
                .build();
        Trajectory right7 = new TrajectoryBuilder(/*new Pose2d(-61.0, 24.0, Math.toRadians(180.0))*/right6.end(), fastConstraints)
                .lineTo(new Vector2d(-53.0, 42.0), new SplineInterpolator(Math.toRadians(-135.0), Math.toRadians(-180.0)))
                .addMarker(new Vector2d(-53.0, 42.0), ()-> {
                    scorer.gripper.setPosition(0);//down
                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(48.0, 42.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .addMarker(new Vector2d(48.0, 42.0), ()->{

                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(1);
                    lift.Arm(-0.5,true);

                    scorer.gripper.setPosition(1);//open
                    lift.runDistance(heightToPivot, 0.5);
                    lift.waitForEncoder();
                    scorer.pivot.setPosition(0.05);//closed
                    lift.Arm(-0.5, true);

                    return Unit.INSTANCE;
                })

                .lineTo(new Vector2d(0.0, 42.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .build();


        waitForStart();

        detector.setOffset(-1f / 8f, 3f / 8f);
        detector.camSetup(hardwareMap);

        //all paths
        Trajectory park = new TrajectoryBuilder(drive.getPoseEstimate(), DriveConstants.BASE_CONSTRAINTS)
                .splineTo(new Pose2d(0.0,39.0))
                .build();

        while (!opModeIsActive() && !isStopRequested()) {
            telemetry.addData("status", "waiting for start command...");
            telemetry.update();
        }

        if (isStopRequested()) return;

        if(opModeIsActive()){
            detector.updateVals();
            vals = detector.getVals();
            telemetry.addData("Values", vals[1] + "   " + vals[0] + "   " + vals[2]);
            telemetry.update();
            intake.hitter.setPosition(1);
            scorer.gripper.setPosition(1); //out
            scorer.pivot.setPosition(0.05); //in

            if(vals[0] == 0){
                drive.followTrajectorySync(mid1);
                drive.followTrajectorySync(mid2);
                drive.followTrajectorySync(mid3);
                drive.followTrajectorySync(mid4);
                drive.followTrajectorySync(mid5);
                drive.followTrajectorySync(mid6);
                drive.followTrajectorySync(mid7);
            } else if(vals [1] == 0){
                drive.followTrajectorySync(left1);
                drive.followTrajectorySync(left2);
                drive.followTrajectorySync(left3);
                drive.followTrajectorySync(left4);
                drive.followTrajectorySync(left5);
                drive.followTrajectorySync(left6);
                drive.followTrajectorySync(left7);
            } else if(vals[2] == 0){
                drive.followTrajectorySync(right1);
                drive.followTrajectorySync(right2);
                drive.followTrajectorySync(right3);
                drive.followTrajectorySync(right4);
                drive.followTrajectorySync(right5);
                drive.followTrajectorySync(right6);
                drive.followTrajectorySync(right7);

            }else{
                drive.followTrajectorySync(park);
            }