package org.firstinspires.ftc.teamcode.BuildZone;

import android.app.Activity;
import android.graphics.Color;
import android.view.View;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DistanceSensor;

import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;

import java.util.Locale;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.Servo;
import java.util.Iterator;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;

import org.firstinspires.ftc.teamcode.DriveClass;

@Autonomous(name="special sauce (blue)!", group="chad")

public class SpecialSauce_BLUE extends LinearOpMode {
    DriveClass dt;
    Servo found1, found2;

    DcMotor intake1, intake2;

    ColorSensor sensorColor;
    DistanceSensor sensorDistance;

    public void runOpMode(){
        // get a reference to the color sensor.
        sensorColor = hardwareMap.get(ColorSensor.class, "colorSense");
        // get a reference to the distance sensor that shares the same name.
        sensorDistance = hardwareMap.get(DistanceSensor.class, "colorSense");

        // hsvValues is an array that will hold the hue, saturation, and value information.
        float hsvValues[] = {0F, 0F, 0F};

        // values is a reference to the hsvValues array.
        final float values[] = hsvValues;

        // sometimes it helps to multiply the raw RGB values with a scale factor
        // to amplify/attentuate the measured values.
        final double SCALE_FACTOR = 255;

        // get a reference to the RelativeLayout so we can change the background
        // color of the Robot Controller app to match the hue detected by the RGB sensor.
        int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
        final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);

        dt = new DriveClass(hardwareMap, this);

        found1 = hardwareMap.get(Servo.class, "foundLeft");
        found2 = hardwareMap.get(Servo.class, "foundRight");

        intake1 = hardwareMap.get(DcMotor.class, "intakeLeft");
        intake2 = hardwareMap.get(DcMotor.class, "intakeRight");
        /* SOME GOOD THINGS TO KNOW:
        1. USE 0.2 POWER FOR TURNING
        2. USE 0.5 POWER FOR FORWARDS / BACKWARDS
        2. DON'T DELETE SHIT IF IT DOESN'T WORK, COMMENT IT
        */

        double factorU;

        dt.initMotors();
        dt.initGyro();
        //
        found1.setPosition(0.0);
        found2.setPosition(0.0);

        waitForStart();
        Color.RGBToHSV((int) (sensorColor.red() * SCALE_FACTOR),
                    (int) (sensorColor.green() * SCALE_FACTOR),
                    (int) (sensorColor.blue() * SCALE_FACTOR),
                    hsvValues);

        /*while(opModeIsActive()) {
            telemetry.addData("ENC", dt.robot.leftBack.getCurrentPosition());
            telemetry.update();
        }*/
        dt.moveBackward(10, 0.3);
        sleep(200);
        dt.strafeRight(12, 0.5);
        sleep(200);
        dt.moveBackward(10, 0.2);
        dt.moveBackward(5, 0.17);
        sleep(300);
        found1.setPosition(0.5);
        found2.setPosition(0.5);
        sleep(1000);
        dt.moveForward(60, 0.5);
        sleep(200);
        found1.setPosition(0.0);
        found2.setPosition(0.0);
        sleep(200);
        dt.strafeLeft(30, 0.5);
        dt.moveBackward(7, 0.5);
        dt.strafeRight(16, 0.4);
        dt.moveBackward(7, 0.5);
        sleep(200);
        dt.strafeLeft(47, 0.5);
        dt.moveBackward(4, 0.2);

        factorU = (sensorColor.red()*sensorColor.green())/
                                            (sensorColor.blue()*sensorColor.blue());

        if(factorU > 2) {
                telemetry.addLine("Tis a stone");
                telemetry.update();
                dt.moveForward(4, 0.4);
                dt.strafeLeft(7, 0.5);
                dt.moveBackward(2, 0.5);

                factorU = (sensorColor.red()*sensorColor.green())/
                                            (sensorColor.blue()*sensorColor.blue());

                if(factorU > 2) {
                        telemetry.addLine("Tis a stone");
                        telemetry.update();

                        dt.moveForward(4, 0.4);
                        dt.strafeLeft(7, 0.5);

                        telemetry.addLine("Tis a skystone (3)");
                        telemetry.update();
                        stoneSequence3();
                } else {
                        telemetry.addLine("Tis a skystone (2)");
                        telemetry.update();
                        stoneSequence2();
                }

        } else {
                telemetry.addLine("Tis a skystone (1)");
                telemetry.update();
                stoneSequence1();
        }
    }

    public void stoneSequence1() {
        dt.moveForward(4, 0.5);
        dt.rotate(-85, 0.3);
        dt.moveBackward(6.5, 0.5);
        dt.strafeRight(16, 0.3);

        intake1.setPower(-0.4);
        intake2.setPower(0.4);

        dt.moveForward(18, 0.3);

        intake1.setPower(0.0);
        intake2.setPower(0.0);

        dt.strafeLeft(12.5, 0.3);
        dt.moveForward(34, 0.7);

        sleep(200);

        intake1.setPower(0.4);
        intake2.setPower(-0.4);
        sleep(1000);
        intake1.setPower(0.0);
        intake2.setPower(0.0);

        sleep(200);

        dt.moveBackward(3, 0.7);

    }

    public void stoneSequence2() {
        dt.moveForward(4, 0.5);
        dt.rotate(-85, 0.3);
        dt.moveBackward(5, 0.5);
        dt.strafeRight(19, 0.3);

        intake1.setPower(-0.4);
        intake2.setPower(0.4);

        dt.moveForward(18, 0.3);

        intake1.setPower(0.0);
        intake2.setPower(0.0);

        dt.strafeLeft(12.5, 0.3);
        dt.moveForward(38, 0.7);

        sleep(200);

        intake1.setPower(0.4);
        intake2.setPower(-0.4);
        sleep(1000);
        intake1.setPower(0.0);
        intake2.setPower(0.0);

        sleep(200);

        dt.moveBackward(6, 0.7);
    }

    public void stoneSequence3() {
        dt.moveForward(4, 0.5);
        dt.rotate(-85, 0.3);
        dt.moveBackward(4, 0.5);
        dt.strafeRight(17, 0.3);

        intake1.setPower(-0.4);
        intake2.setPower(0.4);

        dt.moveForward(16, 0.3);

        intake1.setPower(0.0);
        intake2.setPower(0.0);

        dt.strafeLeft(12.5, 0.3);
        dt.moveForward(50, 0.7);

        sleep(200);

        intake1.setPower(0.4);
        intake2.setPower(-0.4);
        sleep(1000);
        intake1.setPower(0.0);
        intake2.setPower(0.0);

        sleep(200);

        dt.moveBackward(6, 0.7);

    }
}