ff

package org.firstinspires.ftc.teamcode.COMPATITAION;

import android.graphics.Color;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;


@Autonomous(name = "auto beacon-shot_blue", group = "nobody likes you")

public class auto_beaconShot_blue extends LinearOpMode
{

    private enum beaconcolor
    {
        sRed,
        sBlue,
        sUnknown
    }
    private double red, blue;
    private ElapsedTime time = new ElapsedTime();
    private int redtimes = 0;
    private int bluetimes = 0;
    int gyro_angle;
    private beaconcolor m_beaconcolor;
    private HardWare11226 hardware = new HardWare11226();
    boolean runonce;
    final double wheel_peremeter = 12.56;

    @Override
    public void runOpMode() throws InterruptedException {

        hardware.init(hardwareMap);
        runonce = true;

        hardware.gyro_sensor.calibrate();

        while (hardware.gyro_sensor.isCalibrating())
        {
            Thread.sleep(50);
            telemetry.addData("status :", "calibrating");
            telemetry.update();
            idle();
        }

        hardware.leftMotor.setDirection(DcMotor.Direction.FORWARD);
        hardware.rightMotor.setDirection(DcMotor.Direction.REVERSE);
        gyro_angle = hardware.gyro_sensor.getHeading();
        beginf();
        telemetry.addData("Status", "ready to run.");
        telemetry.update();

        waitForStart();
        time.reset();
        time.startTime();

        while (opModeIsActive())
        {
            telemetry.addData("Status: ", "while.");

            if (runonce == true)
            {


                driving(0.2, 28);
                turning(90, 0.6);
                sleep(350);
                driving(24, 0.8);
                turning(-90, -0.6);
                driving(18, 0.8);
                gyro(0.2, 90);
                driving(25, 0.6);
                telemetry.addData("beacon detect :", "start now");
                beaconDetectBlue();
                telemetry.addData("beacon detect :", "detected");
                telemetry.addData("done", "1");
                telemetry.update();
                driving(-31, -1);
                turning(175, 0.6);
                Shooting();
                hardware.collectMotor.setPower(1);
                driving(20, 1);
                runonce = false;
            }

            telemetry.update();
            idle();
        }
        Stop();
    }

    public void turning(int angle, double power) {


        int Target = (int) (((46.4955713 / wheel_peremeter) * 3.111) * angle);

        hardware.leftMotor.setTargetPosition(Target);
        hardware.rightMotor.setTargetPosition(-Target);

        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        hardware.leftMotor.setPower(power);
        hardware.rightMotor.setPower(-power);

        while (hardware.leftMotor.isBusy() && hardware.rightMotor.isBusy() && opModeIsActive()) {
            telemetry.addData("Status: ", "isBusy.");
            telemetry.addData("left pos", hardware.leftMotor.getCurrentPosition());
            telemetry.addData("right pos", hardware.rightMotor.getCurrentPosition());
            telemetry.update();
        }

        Stop();
        beginf();
        sleep(350);

        telemetry.update();
    }

    public void driving(double distance, double power) {

        int Target = (int) ((distance / wheel_peremeter) * 1120);
        hardware.leftMotor.setTargetPosition(Target);
        hardware.rightMotor.setTargetPosition(Target);

        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        hardware.leftMotor.setPower(power);
        hardware.rightMotor.setPower(power);
        while (hardware.rightMotor.isBusy() && opModeIsActive()) {
            telemetry.addData("Status: ", "isBusy.");
            telemetry.addData("left pos", hardware.leftMotor.getCurrentPosition());
            telemetry.addData("right pos", hardware.rightMotor.getCurrentPosition());

        }

        Stop();
        beginf();
        sleep(350);

        telemetry.update();
    }

    public void Stop() {
        hardware.leftMotor.setPower(0);
        hardware.rightMotor.setPower(0);
    }

    public void beaconDetectBlue() throws InterruptedException {
        float hsvValues[] = {0F, 0F, 0F};
        Color.RGBToHSV(hardware.color_sensor_red.red() * 8, hardware.color_sensor_red.green() * 8, hardware.color_sensor_red.blue() * 8, hsvValues);
        time.reset();
        while (time.milliseconds() <= 600 && opModeIsActive()) {
            if (hsvValues[0] <= 40 || hsvValues[0] >= 340) {
                redtimes++;
                telemetry.addData("red :", redtimes);
                telemetry.update();
            } else if (hsvValues[0] <= 240 || hsvValues[0] >= 220) {
                bluetimes++;
                telemetry.addData("blue :", bluetimes);
                telemetry.update();
            }
        }

        if (redtimes > bluetimes) {
            m_beaconcolor = beaconcolor.sRed;
            telemetry.addData("case red :", redtimes);
            telemetry.update();
        } else if (bluetimes > redtimes) {
            m_beaconcolor = beaconcolor.sBlue;
            telemetry.addData("case blue", bluetimes);
            telemetry.update();
        } else {
            m_beaconcolor = beaconcolor.sUnknown;
        }


        telemetry.addData("hue", hsvValues[0]);
        telemetry.addData("redtimes", redtimes);
        driving(5, 0.4);
        driving(5, -0.4);
        telemetry.addData("bluetimes", bluetimes);

        telemetry.update();

        switch (m_beaconcolor) {
            case sBlue:
                telemetry.addData("beacon color detected!!!", " blue");
                telemetry.update();
                break;

            case sRed:
                telemetry.addData("beacon color detected!!!", " red");
                telemetry.update();
                driving(-4, -0.8);
                sleep(4500);
                driving(5, -0.8);
                break;

            case sUnknown:
                telemetry.addData("beacon color NOT detected!!!", " search again for color");
                telemetry.update();
                beaconDetectBlue();
                break;
        }
    }



    public void beginf() {

        hardware.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);


    }

    public void gyro(double power, int idil)
    {

        while ((gyro_angle > idil + 2 || gyro_angle < idil - 2) && opModeIsActive())
        {
            hardware.leftMotor.setPower(power);
            hardware.rightMotor.setPower(-power);

            gyro_angle = hardware.gyro_sensor.getHeading();
            telemetry.addData("angle :", gyro_angle);
            telemetry.update();
        }

        telemetry.addData("angle :", gyro_angle);
        telemetry.update();

        Stop();
        sleep(350);
    }
    private void Shooting() {
        hardware.shootingMotor.setPower(1);
        sleep(1000);
        hardware.shootingServo.setPosition(0);
        sleep(500);
        hardware.shootingServo.setPosition(1);
        hardware.shootingMotor.setPower(0);
        hardware.collectMotor.setPower(-0.8);
        sleep(2000);
        hardware.shootingMotor.setPower(1);
        sleep(1000);
        hardware.shootingServo.setPosition(0);
        sleep(1000);
        hardware.shootingMotor.setPower(0);
        hardware.collectMotor.setPower(0);

    }

}