package sample_camera_opmodes;

import android.graphics.Bitmap;

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

import com.qualcomm.robotcore.hardware.DcMotor;

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

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.eventloop.opmode.OpMode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import com.qualcomm.robotcore.hardware.DcMotor;

import com.qualcomm.robotcore.hardware.DcMotorController;

import com.qualcomm.robotcore.hardware.DcMotorEx;

import com.qualcomm.robotcore.hardware.Servo;

import com.qualcomm.robotcore.util.ElapsedTime;

import for_camera_opmodes.LinearOpModeCamera;

import for_camera_opmodes.OpModeCamera;

/**

* TeleOp Mode

* <p/>

* Enables control of the robot via the gamepad

*/

@Autonomous(name = "DetectColorAuto", group = "ZZOpModeCameraPackage")

//@Disabled

public class DetectColorTest extends LinearOpModeCamera {

int ds2 = 2; // additional downsampling of the image

private int looped = 0;

private long lastLoopTime = 0;

// set to 1 to disable further downsampling

/*

* Code to run when the op mode is first enabled goes here

* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()

*/

@Override

public void runOpMode() {

//telemetry.addLine(" Target Position: " + motorRight.getTargetPosition());

//telemetry.addLine(" Current Position: " + motorRight.getCurrentPosition());

int counts = 1050;

int smallTurn = 425;

int initialForward = 3143;

int longBeacon = 1100;

int shortBeacon = 180;

int betweenBeacons = 2950;

telemetry.addLine("446");

goForwards((initialForward)); //Initial Forwards

sleep(25);

turnLeft(smallTurn); //Turn Left 45

sleep(25);

goForwards(betweenBeacons); //Go to far beacon

sleep(25);

turnRight(counts); //Turn Right 90

sleep(25);

slowForwards(longBeacon);

sleep(25);

goBackwards(shortBeacon);

sleep(600);

//telemetry.addLine("Color " + color());

if(color() == 0)

{

slowForwards(shortBeacon);

sleep(25);

goBackwards(longBeacon);

}

//telemetry.addLine("DONE!");

/* turnRight(counts); //Right Turn to Beacon

goForwards(longBeacon); //Forwards to hit Beacon

goBackwards(shortBeacon); //Go back from beacon

//Insert If Statement

goBackwards(longBeacon-shortBeacon); //Go Back Further from Beacon

turnRight(counts); //Turn Right

goForwards(betweenBeacons); //go Between Beacons

turnLeft(counts); //Left to Beacon

goForwards(longBeacon); //Hit Beacon

goBackwards(shortBeacon); //Back from Beacon

//Insert If Statement

*/

}

public int color() {

setCameraDownsampling(8);

// parameter determines how downsampled you want your images

// 8, 4, 2, or 1.

// higher number is more downsampled, so less resolution but faster

// 1 is original resolution, which is detailed but slow

// must be called before super.init sets up the camera

super.init(); // inits camera functions, starts preview callback

int temp = 3;

// only do this if an image has been returned from the camera

int redValue = 0;

int blueValue = 0;

int greenValue = 0;

// get image, rotated so (0,0) is in the bottom left of the preview window

Bitmap rgbImage;

rgbImage = convertYuvImageToRgb(yuvImage, width, height, ds2);

for (int x = 0; x < rgbImage.getWidth(); x++) {

for (int y = 0; y < rgbImage.getHeight(); y++) {

int pixel = rgbImage.getPixel(x, y);

redValue += red(pixel);

blueValue += blue(pixel);

greenValue += green(pixel);

}

}

int color = highestColor(redValue, greenValue, blueValue);

String colorString = "";

switch (color) {

case 0:

temp = 0;

break;

case 1:

temp = 2;

break;

case 2:

temp = 1;

}

//telemetry.addData("Color:", "Color detected is: " + colorString);

return temp;

}

void goBackwards(int counts) {

DcMotor motorRight = hardwareMap.dcMotor.get("rightMotors");

DcMotor motorLeft = hardwareMap.dcMotor.get("leftMotors");

motorRight.setMode(DcMotor.RunMode.RESET_ENCODERS);

motorRight.setTargetPosition(counts);

motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);

motorRight.setPower(.6);

motorLeft.setPower(-.6);

while (motorRight.isBusy()) {

}

motorRight.setPower(0);

motorLeft.setPower(0);

}

void goForwards(int counts) {

DcMotor motorRight = hardwareMap.dcMotor.get("rightMotors");

DcMotor motorLeft = hardwareMap.dcMotor.get("leftMotors");

motorRight.setMode(DcMotor.RunMode.RESET_ENCODERS);

motorRight.setTargetPosition(-counts);

motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);

motorRight.setPower(-.6);

motorLeft.setPower(.6);

while (motorRight.isBusy()) {

}

motorRight.setPower(0);

motorLeft.setPower(0);

}

void slowForwards(int counts) {

DcMotor motorRight = hardwareMap.dcMotor.get("rightMotors");

DcMotor motorLeft = hardwareMap.dcMotor.get("leftMotors");

motorRight.setMode(DcMotor.RunMode.RESET_ENCODERS);

motorRight.setTargetPosition(-counts);

motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);

motorRight.setPower(-.2);

motorLeft.setPower(.2);

while (motorRight.isBusy()) {

}

motorRight.setPower(0);

motorLeft.setPower(0);

}

void turnLeft(int counts) {

DcMotor motorRight = hardwareMap.dcMotor.get("rightMotors");

DcMotor motorLeft = hardwareMap.dcMotor.get("leftMotors");

motorRight.setMode(DcMotor.RunMode.RESET_ENCODERS);

motorRight.setTargetPosition(-counts);

motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);

motorRight.setPower(-.7);

motorLeft.setPower(-.7);

while (motorRight.isBusy()) {

}

motorRight.setPower(0);

motorLeft.setPower(0);

}

void turnRight(int counts) {

DcMotor motorRight = hardwareMap.dcMotor.get("rightMotors");

DcMotor motorLeft = hardwareMap.dcMotor.get("leftMotors");

motorRight.setMode(DcMotor.RunMode.RESET_ENCODERS);

motorRight.setTargetPosition(counts);

motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);

motorRight.setPower(.7);

motorLeft.setPower(.7);

while (motorRight.isBusy()) {

}

motorRight.setPower(0);

motorLeft.setPower(0);

}

void sleep(int input) {

for(int i = 0; i<input; i++)

{

for(int a = 0; a<1000000; a++)

{

}

}

}