auto_2

package org.firstinspires.ftc.teamcode;

import android.graphics.Color;

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


@Autonomous(name = "ALL", group = "nobody likes you")
@Disabled
public class auto_2 extends LinearOpMode
{

    private enum beaconcolor
    {
        sRed,
        sBlue,
        sUnknown
    }

    private ElapsedTime time = new ElapsedTime();
    private int redtimes = 0;
    private int bluetimes = 0;
    private beaconcolor m_beaconcolor;
    private HardWare11226 m_hardware = new HardWare11226();
    boolean runonce;
    final double wheel_peremeter = 12.56;

    @Override
    public void runOpMode() throws InterruptedException {

        m_hardware.init(hardwareMap);

        runonce = true;

        m_hardware.leftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
        m_hardware.rightMotor.setDirection(DcMotor.Direction.REVERSE);

        beginf();

        telemetry.addData("Status", "ready to run.");
        telemetry.update();

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

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

            if (runonce == true)
            {

                driving(49, 0.8);
                turning(-90, -0.4);
                driving(35, 0.5);
                telemetry.addData("done","2");
                telemetry.update();
                driving(-8, -0.6);
                driving(9, 0.4);
                telemetry.addData("done","3");
                telemetry.update();
                driving(-32, -0.8);
                turning(90, 0.4);
                telemetry.addData("done","4");
                telemetry.update();
                driving(44, 0.8);
                telemetry.addData("done","6");
                telemetry.update();
                turning(-90, -0.4);
                telemetry.addData("Status: ", "done.");
                driving(35, 0.3);
                driving(-8, -0.4);
                driving(9, 0.3);
                driving(-2, 0.2);
                beaconDetectRed();
                driving(-8, 0.3);
                runonce = false;
            }

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

    public void turning(int angle, double power) {


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

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

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

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

        while (m_hardware.leftMotor.isBusy() && m_hardware.rightMotor.isBusy()) {
            telemetry.addData("Status: ", "isBusy.");
            telemetry.addData("left pos", m_hardware.leftMotor.getCurrentPosition());
            telemetry.addData("right pos", m_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);
        m_hardware.leftMotor.setTargetPosition(Target);
        m_hardware.rightMotor.setTargetPosition(Target);

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

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

        }

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

        telemetry.update();
    }

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


    public void beaconDetectRed() throws InterruptedException
    {
        float hsvValues[] = {0F, 0F, 0F};
        Color.RGBToHSV(m_hardware.color_sensor_red.red() * 8, m_hardware.color_sensor_red.green() * 8, m_hardware.color_sensor_red.blue() * 8, hsvValues);
        time.reset();
        while (time.milliseconds() <= 600) {
            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");
                driving(-4, -0.8);
                sleep(4500);
                turning(-10, -0.4);
                driving(-5, -0.8);

                break;

            case sRed:
                telemetry.addData("beacon color detected!!!", " red");
                break;

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


    public void beginf()
    {

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

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


    }

    public void gyro(double l_power, double r_power,  int idil)
    {

    }
}