auto

package org.firstinspires.ftc.teamcode;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.path.LineSegment;
import com.acmerobotics.roadrunner.path.heading.ConstantInterpolator;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.TrajectoryLoader;
import com.disnodeteam.dogecv.detectors.skystone.SkystoneDetector;
import com.disnodeteam.dogecv.filters.GrayscaleFilter;
import com.disnodeteam.dogecv.filters.LeviColorFilter;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveREV;
import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveREVOptimized;
import org.firstinspires.ftc.teamcode.util.AssetsTrajectoryLoader;
import org.openftc.easyopencv.OpenCvCameraRotation;
import org.openftc.easyopencv.OpenCvWebcam;

import java.io.File;
import java.io.IOException;

import static org.firstinspires.ftc.teamcode.OpenCvShit.yellowt;

/*
 * This is a simple routine to test translational drive capabilities.
 */
@Config
@Autonomous(name ="RR si OpenCv",group = "drive")

public class RR_OpenCv extends LinearOpMode {
    public static double DISTANCE = 30;
    private OpenCvWebcam webcam;
    private SkystoneDetector skystoneDetector;
    public static double yellow = 100;
    public static int gray = 40;
    public static String position = null;
    public ElapsedTime runtime = new ElapsedTime();



    @Override
    public void runOpMode() throws InterruptedException {
        SampleMecanumDriveREVOptimized robot = new SampleMecanumDriveREVOptimized(hardwareMap);
        webcam = new OpenCvWebcam(hardwareMap.get(WebcamName.class,"camera porno"));
        webcam.openCameraDevice();
        skystoneDetector = new SkystoneDetector();
        skystoneDetector.blackFilter = new GrayscaleFilter(0, gray);
        skystoneDetector.yellowFilter = new LeviColorFilter(LeviColorFilter.ColorPreset.YELLOW, yellow);
        webcam.setPipeline(skystoneDetector);
        webcam.startStreaming(640,480, OpenCvCameraRotation.UPRIGHT);
        FtcDashboard ftcDashboard = FtcDashboard.getInstance();
        ftcDashboard.startCameraStream(webcam, 30);


        waitForStart();


        if (isStopRequested()) return;
        robot.led.enableLed(false);
        position = detecteaza(robot);
        runtime.reset();
        while (runtime.seconds() < 1.5){
            telemetry.addData("pozitie: ", position);
            telemetry.update();
        }
        if(position == "dreapta") tr_dr(robot);
        else if(position == "mijloc") tr_mij(robot);
        else tr_stg(robot);

    }

    //cod detectare skystone

    public String detecteaza(SampleMecanumDriveREVOptimized robot){
        double x_position =0 , y_position= 0;
        sleep(200);

        runtime.reset();
        while (runtime.seconds() < 2.0){
            x_position = skystoneDetector.getScreenPosition().x;
            y_position = skystoneDetector.getScreenPosition().y;
        }

        if( x_position < 330){
            return "stanga";
        }else if( x_position > 380 && x_position< 500){
            return "mijloc";
        }else return "dreapta";
    }

    public void tr_dr(SampleMecanumDriveREVOptimized robot){

    }
    public void tr_mij(SampleMecanumDriveREVOptimized robot){

    }
    public void tr_stg(SampleMecanumDriveREVOptimized robot){
        Trajectory traj0 = robot.trajectoryBuilder()
                .back(7.5)
                .strafeLeft(55)
                .build();
        Trajectory traj1 = robot.trajectoryBuilder()
                .reverse()
                .strafeRight(6.5)
                .reverse()
                .back(135)
                .strafeLeft(6)
                .build();
        Trajectory traj2 = robot.trajectoryBuilder()
                .strafeRight(2.5)
                .forward(200)
                .build();

        robot.brat.setPosition(0);
        robot.claw.setPosition(0.5);//in sus
        sleep(500);
        robot.followTrajectorySync(traj0);
        robot.claw.setPosition(0);
        sleep(650);
        robot.brat.setPosition(0.5);
        sleep(400);
        robot.followTrajectorySync(traj1);
        robot.brat.setPosition(0.05);
        sleep(200);
        robot.claw.setPosition(0.6);
        robot.brat.setPosition(0.5);
        sleep(600);
        robot.followTrajectorySync(traj2);

    }


}