import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;

@Autonomous(name = "bBlueAutoV1", group = "V1")
public class bBlueAutoV1 extends OpMode {

    public Telemetry telemetry;
    public Follower robot;
    public PathChain path;

    @Override
    public void init() {
        robot = new Follower(hardwareMap);

        PathBuilder builder = new PathBuilder();

       path = builder
                .addPath(
                        // Line 1
                        new BezierLine(
                                new Point(9.757, 84.983, Point.CARTESIAN),
                                new Point(28.573, 76.302, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 2
                        new BezierLine(
                                new Point(28.573, 76.302, Point.CARTESIAN),
                                new Point(36.203, 76.140, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 3
                        new BezierLine(
                                new Point(36.203, 76.140, Point.CARTESIAN),
                                new Point(35.067, 35.716, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 4
                        new BezierLine(
                                new Point(35.067, 35.716, Point.CARTESIAN),
                                new Point(73.705, 34.742, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 5
                        new BezierLine(
                                new Point(73.705, 34.742, Point.CARTESIAN),
                                new Point(73.705, 24.839, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 6
                        new BezierLine(
                                new Point(73.705, 24.839, Point.CARTESIAN),
                                new Point(7.630, 26.462, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 7
                        new BezierLine(
                                new Point(7.630, 26.462, Point.CARTESIAN),
                                new Point(64.126, 22.728, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 8
                        new BezierLine(
                                new Point(64.126, 22.728, Point.CARTESIAN),
                                new Point(63.964, 13.150, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 9
                        new BezierLine(
                                new Point(63.964, 13.150, Point.CARTESIAN),
                                new Point(12.338, 15.260, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 10
                        new BezierLine(
                                new Point(12.338, 15.260, Point.CARTESIAN),
                                new Point(63.802, 13.150, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 11
                        new BezierLine(
                                new Point(63.802, 13.150, Point.CARTESIAN),
                                new Point(63.639, 11.689, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 12
                        new BezierLine(
                                new Point(63.639, 11.689, Point.CARTESIAN),
                                new Point(12.014, 11.689, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 13
                        new BezierLine(
                                new Point(12.014, 11.689, Point.CARTESIAN),
                                new Point(62.665, 30.196, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 14
                        new BezierLine(
                                new Point(62.665, 30.196, Point.CARTESIAN),
                                new Point(13.312, 51.463, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 15
                        new BezierLine(
                                new Point(13.312, 51.463, Point.CARTESIAN),
                                new Point(16.234, 103.738, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 16
                        new BezierLine(
                                new Point(16.234, 103.738, Point.CARTESIAN),
                                new Point(68.023, 108.284, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 17
                        new BezierLine(
                                new Point(68.023, 108.284, Point.CARTESIAN),
                                new Point(68.185, 121.109, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 18
                        new BezierLine(
                                new Point(68.185, 121.109, Point.CARTESIAN),
                                new Point(21.754, 119.811, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 19
                        new BezierLine(
                                new Point(21.754, 119.811, Point.CARTESIAN),
                                new Point(11.526, 129.227, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 20
                        new BezierLine(
                                new Point(11.526, 129.227, Point.CARTESIAN),
                                new Point(72.568, 111.856, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 21
                        new BezierLine(
                                new Point(72.568, 111.856, Point.CARTESIAN),
                                new Point(58.607, 128.902, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 22
                        new BezierLine(
                                new Point(58.607, 128.902, Point.CARTESIAN),
                                new Point(11.364, 130.850, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 23
                        new BezierLine(
                                new Point(11.364, 130.850, Point.CARTESIAN),
                                new Point(58.931, 128.577, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 24
                        new BezierLine(
                                new Point(58.931, 128.577, Point.CARTESIAN),
                                new Point(58.769, 133.123, Point.CARTESIAN)
                        )
                )

                .addPath(
                        // Line 25
                        new BezierLine(
                                new Point(58.769, 133.123, Point.CARTESIAN),
                                new Point(13.475, 133.935, Point.CARTESIAN)
                        )
                ).build();
        ;
    }


    @Override
    public void loop() {
        robot.update();
        if (robot.atParametricEnd())
            robot.followPath(path);
        robot.telemetryDebug(telemetry);
    }
}