package com.qualcomm.ftcrobotcontroller.opmodes;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.GyroSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;

public class auto3_wait extends LinearOpMode{
    //motors for drive train
    GyroSensor gsens;
    double default_left = -0.3;
    double default_right = 0.3;
    double motorleft = default_left;
    double motorright = default_right;

    DcMotor motorRF;
    DcMotor motorRB;
    DcMotor motorLF;
    DcMotor motorLB;

    private float servo_rate = 0.5f;

    //motors for arm
    DcMotor extendArm1;
    DcMotor extendArm2;
    DcMotor retractArm;
    DcMotor raiseArm;

    //servos for other shit
    Servo boxRotateServo;
    Servo putterServo;
    Servo trapdoorServo;
    Servo sweeperServo;
    Servo putterServo2;
    Servo climberServo;

    public void preciseSleep(int duration){
        long currentTime = System.currentTimeMillis();
        long startTime = currentTime;
        while(currentTime < (startTime + duration)){
            currentTime = System.currentTimeMillis();
        }
    }

    static double sweeperVal = 0.5;

    @Override
    public void runOpMode() throws InterruptedException{
        motorRF = hardwareMap.dcMotor.get("motorRF");
        motorRB = hardwareMap.dcMotor.get("motorRB");
        motorLF = hardwareMap.dcMotor.get("motorLF");
        motorLB = hardwareMap.dcMotor.get("motorLB");

        extendArm1 = hardwareMap.dcMotor.get("extend1");
        extendArm2 = hardwareMap.dcMotor.get("extend2");

        raiseArm = hardwareMap.dcMotor.get("raise");
        retractArm = hardwareMap.dcMotor.get("retract");

        boxRotateServo = hardwareMap.servo.get("boxRotate");
        sweeperServo = hardwareMap.servo.get("sweeper");
        trapdoorServo = hardwareMap.servo.get("trapDoor");
        putterServo = hardwareMap.servo.get("putter");
        putterServo2 = hardwareMap.servo.get("putter2");
        climberServo = hardwareMap.servo.get("climber");

        gsens = hardwareMap.gyroSensor.get("gyro");
        //telemetry.addData("rotation", gsens.getRotation());

        motorRF.setPower(0.0);
        motorRB.setPower(0.0);
        motorLF.setPower(0.0);
        motorLB.setPower(0.0);
        extendArm1.setPower(0.0);
        extendArm2.setPower(0.0);
        retractArm.setPower(0.0);
        raiseArm.setPower(0.0);

        boxRotateServo.setPosition(0.5);
        sweeperServo.setPosition(0.5);
        trapdoorServo.setPosition(0.5);
        climberServo.setPosition(1);
        putterServo.setPosition(1);
        putterServo2.setPosition(0);

        waitForStart();
        preciseSleep(15000);

        motorLB.setPower(default_left);
        motorLF.setPower(default_left);
        motorRB.setPower(default_right);
        motorRF.setPower(default_right);
        //Move sweeper backwards continuously
        sweeperServo.setPosition(1);
        int counter = 0;
        while(counter < 1200) {
            double delta = (gsens.getRotation() - 595.0);
            if (delta > 0.0 && motorright < default_right + 0.1) {
                //Turning to the right
                motorright = motorright + 0.01;
                motorleft = default_left;
            } else if (delta < 0.0 && motorLB.getPower() > default_left - 0.1) {
                //Turning to the left
                motorleft = motorleft - 0.01;
                motorright = default_right;
            } else {
                motorleft = default_left;
                motorright = default_right;
            }
            motorLB.setPower(motorleft);
            motorLF.setPower(motorleft);
            motorRB.setPower(motorright);
            motorRB.setPower(motorright);
            counter = counter + 1;
            telemetry.addData("rotation", delta);
            telemetry.addData("counter", counter);
            telemetry.addData("motorleft", motorleft);
            telemetry.addData("motorright", motorright);
            telemetry.addData("actual_left", motorLB.getPower());
            telemetry.addData("actual_right", motorRB.getPower());
            telemetry.addData("default_left", default_left);
            telemetry.addData("default_right", default_right);
            telemetry.addData("rot", gsens.getRotation());
            sleep(1);
        }
        sleep(10);
        //Stop moving sweeper
        sweeperServo.setPosition(0.5);
        climberServo.setPosition(0);
        motorLB.setPower(0.0);
        motorLF.setPower(0.0);
        motorRB.setPower(0.0);
        motorRF.setPower(0.0);
        telemetry.addData("left_busy", motorLB.getPower());
        telemetry.addData("right busy", motorRB.getPower());
        preciseSleep(1500);
        climberServo.setPosition(1);
    }
}