package org.usfirst.frc.team6083.robot;

import java.io.PipedReader;

import org.opencv.core.Rect;
import org.opencv.imgproc.Imgproc;

import edu.wpi.cscore.AxisCamera;
import edu.wpi.cscore.CvSource;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.vision.VisionThread;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	final String defaultAuto = "Default";
	final String customAuto = "My Auto";
	String autoSelected;
	SendableChooser<String> chooser = new SendableChooser<>();
	static final int width = 640, height = 480;
	double centerX, left, length;
	private VisionThread visionThread;
	Object imglock = new Object();
	VictorSP motor1 = new VictorSP(1);
	VictorSP motor2 = new VictorSP(2);
	AxisCamera camera = new AxisCamera("Axis Camera 1", "axis-camera1.local");
	private static double speedl, speedr, max_turn = 0.2;
	private static int item_width = 75;

	CvSource output;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		chooser.addDefault("Def	ault Auto", defaultAuto);
		chooser.addObject("My Auto", customAuto);
		SmartDashboard.putData("Auto choices", chooser);

		camera.setResolution(width, height);
		output = CameraServer.getInstance().putVideo("Processed: ", 640, 360);

		visionThread = new VisionThread(camera, new GripPipeline(), pipeline -> {
			if (!pipeline.filterContoursOutput().isEmpty()) {
				Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0));
				synchronized (imglock) {
					centerX = r.x + (r.width / 2);
					SmartDashboard.putNumber("centerX", centerX);
					left = r.x;
					length = r.width;
				}

			}
			output.putFrame(pipeline.rgbThresholdOutput());
		});
		visionThread.start();

		SmartDashboard.putNumber("turn", 0);
		SmartDashboard.putNumber("forward", 0);
		SmartDashboard.putNumber("x", 0);
		SmartDashboard.putNumber("speedl", 0);
		SmartDashboard.putNumber("speedr", 0);
		SmartDashboard.putNumber("turn_speed", 0);
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select
	 * between different autonomous modes using the dashboard. The sendable
	 * chooser code works with the Java SmartDashboard. If you prefer the
	 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
	 * getString line to get the auto name from the text box below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the
	 * switch structure below with additional strings. If using the
	 * SendableChooser make sure to add them to the chooser code above as well.
	 */
	@Override
	public void autonomousInit() {
		autoSelected = chooser.getSelected();
		// autoSelected = SmartDashboard.getString("Auto Selector",
		// defaultAuto);
		System.out.println("Auto selected: " + autoSelected);
	}

	/**
	 * This function is called periodically during autonomous
	 */
	@Override
	public void autonomousPeriodic() {
		double centerX, left, length, forward;
		synchronized (imglock) {
			centerX = this.centerX;
			left = this.left;
			length = this.length;
		}
		double turn = centerX - (width / 2);
		if (length > 5) {
				forward=length/item_width;
		} else {
			forward = 0;
		}
		if(Math.abs(turn * 0.001)>max_turn){
			speedl=turn/Math.abs(turn)*0.2;
			speedr=turn/Math.abs(turn)*-0.2;
		}else if(Math.abs(turn * 0.001)<0.07){
			speedl =0;
			speedr =0;
		}else{
			speedl =turn * 0.001;
			speedr =-turn * 0.001;
		}
		if(forward>1.05){
			speedl-=0.15;
			speedr-=0.15;
		}else if(forward<0.9&&forward>0.5){
			speedl+=0.15;
			speedr+=0.15;
		}

		motor1.set(speedl);//+ forward
		motor2.set(-speedr);//+ back
		SmartDashboard.putNumber("turn", turn);
		SmartDashboard.putNumber("forward", forward);
		SmartDashboard.putNumber("turn_speed", turn*0.001);
		SmartDashboard.putNumber("centerX", centerX);
		SmartDashboard.putNumber("speedl", speedl);
		SmartDashboard.putNumber("speedr", speedr);
	}

	/**
	 * This function is called periodically during operator control
	 */
	@Override
	public void teleopPeriodic() {

	}

	/**
	 * This function is called periodically during test mode
	 */
	@Override
	public void testPeriodic() {
	}
}