package org.firstinspires.ftc.teamcode;

/*
 * AutonomousRed: autonomous software to knock off a ball (red alliance version).
 * All of the code that runs in the autonomous period is in runOpMode().
 * Unlike teleop, there is no "loop" method; code runs in a straight line from top to bottom.
 *
 * multiple authors, February 2018
 */

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;


@Autonomous(name="Autonomous: KnockOff Red", group="WoodyBot")
public class Autonomous_Red extends LinearOpMode {
    WoodyBot robot = new WoodyBot();

    @Override
    public void runOpMode() {
        // initialize the robot hardware
        robot.init(hardwareMap);

        // initialize Vuforia
        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
        parameters.vuforiaLicenseKey = robot.vKey;
        parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
        VuforiaLocalizer vuforia = ClassFactory.createVuforiaLocalizer(parameters);
        RelicRecoveryVuMark pictograph = null;

        // load the pictograph dataset
        VuforiaTrackables relicTrackables = vuforia.loadTrackablesFromAsset("RelicVuMark");
        VuforiaTrackable relicTemplate = relicTrackables.get(0);

        // get visible pictograph
        relicTrackables.activate();
        for(int i = 0; i < 20; i++) pictograph = RelicRecoveryVuMark.from(relicTemplate);

        // send telemetry message to signify robot waiting
        telemetry.addData("Status", "Slouching");
        telemetry.addData("VuMark", "%s visible", pictograph);
        telemetry.update();

        waitForStart(); // wait for match to start (driver presses PLAY)

        // ---------- match code ----------

        robot.ColorSense.setPosition(0.65); // arm straight up
        sleep(1000);
        robot.TwistyThingy.setPosition(0.13); // rotate sensor into position
        sleep(1000);
        robot.ColorSense.setPosition(0.03); // arm down
        sleep(2000);
        int red = robot.CS.red();
        int blue = robot.CS.blue();
        sleep(500);

        if(red > blue) { // facing red ball - move left/CCW to knock off red ball
            telemetry.addData("Color", "blue");
            telemetry.update();
            sleep(1000);
            robot.TwistyThingy.setPosition(0);
            sleep(1500);
            robot.ColorSense.setPosition(0.6); // retract arm
            sleep(1500);
        }
        if(red < blue) { // facing blue ball - move right/CW to knock off blue ball
            telemetry.addData("Color","red");
            telemetry.update();
            sleep(1000);
            robot.TwistyThingy.setPosition(0.3); // rotate right
            sleep(1500);
            robot.ColorSense.setPosition(0.6); // retract arm
            sleep(1500);
        }
        robot.TwistyThingy.setPosition(0);
        robot.ColorSense.setPosition(.61);
        sleep(300);

        robot.FrontMotorRight.setPower(-.25);
        robot.FrontMotorLeft.setPower(-.25);

        // fixes hang between autonomous and teleop
        robot.CS.enableLed(false);
        sleep(400);

        robot.FrontMotorRight.setPower(0);
        robot.FrontMotorLeft.setPower(0);

        // ---------- vision code ----------

        // take different actions depending on the pictograph visible at start of autonomous
        switch(pictograph) {
            case LEFT:
                break;

            case CENTER:
                robot.FrontMotorRight.setPower(.25);
                robot.FrontMotorLeft.setPower(.35);
                sleep(800);
                break;

            case RIGHT:
                break;

            default:
                // code if nothing is detected...
                break;
        }

        robot.FrontMotorRight.setPower(0);
        robot.FrontMotorLeft.setPower(0);

        sleep(500); // a final sleep before switching to teleop
    }
}

@@ -1663,24 +1663,281 @@
 es.get(0);%0A%0A
+        // send telemetry message to signify robot waiting%0A        telemetry.addData(%22Status%22, %22Slouching%22);%0A        telemetry.update();%0A%0A        waitForStart(); // wait for match to start (driver presses PLAY)%0A%0A        // ---------- match code ----------%0A%0A
         // g
@@ -2021,17 +2021,18 @@
  0; i %3C 
-2
+50
 0; i++) 
@@ -2088,118 +2088,8 @@
 te);
-%0A%0A        // send telemetry message to signify robot waiting%0A        telemetry.addData(%22Status%22, %22Slouching%22);
 %0A   
@@ -2172,33 +2172,32 @@
 y.update();%0A
-%0A
         
 waitForStart
@@ -2188,117 +2188,20 @@
     
-waitForStart(); // wait for match to start (driver presses PLAY)%0A%0A        // ---------- match code ----------
+sleep(1000);
 %0A%0A  

change order of Vuforia pictograph grab