package org.firstinspires.ftc.teamcode.BuildZone;

import com.qualcomm.hardware.bosch.BNO055IMU;
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="RED (Found & Park): Wall Park", group="chad")

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

    public void runOpMode(){
        dt = new DriveClass(hardwareMap, this);

        found1 = hardwareMap.get(Servo.class, "foundLeft");
        found2 = hardwareMap.get(Servo.class, "foundRight");
        /* 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
        */

        dt.initMotors();
        dt.initGyro();
        //
        waitForStart();

        found1.setPosition(0.0);
        found2.setPosition(0.0);
        sleep(1000);
        dt.moveBackward(10, 0.3);
        sleep(200);
        dt.strafeLeft(8, 0.5);
        sleep(200);
        dt.moveBackward(7, 0.2);
        dt.moveBackward(4, 0.17);
        sleep(300);
        found1.setPosition(0.5);
        found2.setPosition(0.5);
        sleep(2000);
        dt.moveForward(55, 0.5);
        sleep(200);
        found1.setPosition(0.0);
        found2.setPosition(0.0);
        sleep(2000);
        dt.strafeRight(35, 0.5);
        dt.moveBackward(7, 0.5);
        dt.strafeLeft(11, 0.3);
        dt.strafeRight(11, 0.3);
        dt.moveForward(12, 0.5);
        dt.strafeRight(20, 0.5);
    }
}