package org.usfirst.frc.team2503.r2016.component;

import org.usfirst.frc.team2503.lib.util.WarriorMath;
import org.usfirst.frc.team2503.r2016.input.Data;

import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Servo;

public class CameraMount extends Component {

	public static class CameraLights extends Relay {

		public CameraLights(int channel) {
			super(channel);
		}

		public CameraLights(int channel, Direction direction) {
			super(channel, direction);
		}

	}

	public enum CameraMountMode {
		UNKNOWN,
		LOOKING,
		TARGETING,
		INTAKE,
		BACKUP,
		HOOKING
	}

	public CameraLights lights;

	public Servo horizontal;
	public Servo vertical;

	private double x;
	private double y;

	private CameraMountMode mode = CameraMountMode.UNKNOWN;

	public void setMode(CameraMountMode mode) { this.mode = mode; }
	public CameraMountMode getMode() { return this.mode; }

	public void tweak(double horizontal, double vertical) {
		this.x = WarriorMath.gate(0.0, (this.x + (horizontal * 0.01)), 1.0);
		this.y = WarriorMath.gate(0.0, (this.y + (vertical * 0.01)), 1.0);
//		this.x = Math.max(0.0, Math.min(1.0, this.x + (horizontal * 0.01)));
//		this.y = Math.max(0.0, Math.min(1.0, this.y + (vertical * 0.01)));
	}

	public void tick(Data data) {
		switch(this.mode) {
		case LOOKING:
			/* this.x and this.y should already have been set by tweak(), so
			 * don't modify them.
			 */
			this.lights.set(Relay.Value.kOn);
			break;

		case INTAKE:
			this.lights.set(Relay.Value.kOn);
			this.x = 1.0;
			this.y = 0.3;
			break;

		case BACKUP:
			this.lights.set(Relay.Value.kOn);
			this.x = 0.0;
			this.y = 0.4;
			break;

		case HOOKING:
			this.lights.set(Relay.Value.kOn);
			this.x = 0.0;
			this.y = 0.6;
			break;

		case TARGETING:
			this.lights.set(Relay.Value.kOn);
			this.x = 1.0;
			this.y = 0.4;
			break;

		case UNKNOWN:
		default:
			this.lights.set(Relay.Value.kOff);
			this.x = 1.0;
			this.y = 0.4;
			break;
		}

		this.horizontal.set(this.x);
		this.vertical.set(this.y);
	}

	public CameraMount(Servo horizontal, Servo vertical, CameraLights lights) {
		this.horizontal = horizontal;
		this.vertical = vertical;
		this.lights = lights;
	}

}

@@ -565,23 +565,32 @@
 CKUP,%0A%09%09
-HOOKING
+STRAIGHT_FORWARD
 %0A%09%7D%0A%0A%09pu
@@ -1083,16 +1083,18 @@
 , 1.0);%0A
+%09%09
 //%09%09this
@@ -1158,16 +1158,18 @@
 .01)));%0A
+%09%09
 //%09%09this
@@ -1535,17 +1535,17 @@
 s.y = 0.
-3
+2
 ;%0A%09%09%09bre
@@ -1624,33 +1624,34 @@
 ;%0A%09%09%09this.y = 0.
-4
+75
 ;%0A%09%09%09break;%0A%0A%09%09c
@@ -1658,15 +1658,24 @@
 ase 
-HOOKING
+STRAIGHT_FORWARD
 :%0A%09%09
@@ -1713,33 +1713,33 @@
 n);%0A%09%09%09this.x = 
-0
+1
 .0;%0A%09%09%09this.y = 
@@ -1740,17 +1740,17 @@
 s.y = 0.
-6
+4
 ;%0A%09%09%09bre
@@ -1832,33 +1832,34 @@
 ;%0A%09%09%09this.y = 0.
-4
+55
 ;%0A%09%09%09break;%0A%0A%09%09c
@@ -2026,16 +2026,24 @@
 t(this.y
+ - 0.05d
 );%0A%09%7D%0A%0A%09

Clean up CameraMount.java