package org.usfirst.frc.team4272.robot2016;

import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc.team4272.robotlib.PushButton;
import org.usfirst.frc.team4272.robotlib.RollingAvg;

public class Teleop {
	private final HwRobot robot;
	private final double armSoftStop = 95;//295;

	private final PushButton shootButton = new PushButton();
	private final Timer time = new Timer();
	private double lastTime;
	private double currentTime;
	private boolean state = false;
	private PIDSource lRate, rRate;
	private Preferences prefs = Preferences.getInstance();

	public Teleop(HwRobot robot) {
		this.robot = robot;
		this.lRate = new RollingAvg(5, robot.lDriveE);
		this.rRate = new RollingAvg(5, robot.rDriveE);
	}

	private static double jsScale(Joystick j) {
		double y = j.getY();/* +:forward; -:backward */
		double z = j.getZ();/* +:more-sensitive; -:less-sensitive */
		return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
	}

	private static double bound(double min, double v, double max) {
		return Math.min(Math.max(v, min), max);
	}

	private static double oneIf(boolean b) {
		return b ? 1 : 0;
	}
	
	public Control run(Control control, HwOI oi) {
		/* Drive */
		control.lDrive = -jsScale(oi.lStick);
		control.rDrive = -jsScale(oi.rStick);

		/* shifting */
		double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28);
		double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28);
		if (oi.lStick.getTrigger()) {
			control.highGear = false;
		} else if (oi.rStick.getTrigger()) {
			control.highGear = true;
		} else {
			double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2);
			if (!control.highGear) {
				if (speed > shiftUp)
					control.highGear = true;
			} else {
				if (speed < shiftDown)
					control.highGear = false;
			}
		}

		/* auto gear wiggle */
		if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) { 
			currentTime = Timer.getMatchTime();
			if(currentTime-lastTime > 0.15){
				state = !state;
				lastTime = currentTime;
			}
			if (state){
				control.lDrive = 0.5;
				control.rDrive = 0;
			}
			else if(!state){
				control.lDrive = 0;
				control.rDrive = 0.5;
			}
		}

		/* climber */
		if(oi.xbox.getRawAxis(1) > 0.2){
			control.climber = oi.xbox.getRawAxis(1);
		} else if (oi.xbox.getRawAxis(1) < -0.2) {
			control.climber = oi.xbox.getRawAxis(1);
		} else {
			control.climber = 0;
		}

		/* gear place */
		if (oi.xbox.getAButton()) {
			control.gedOut = true;
		}
		if (oi.xbox.getBButton()) {
			control.gedOut = false;
		}
		
		/* camera */
		if(oi.xbox.getTriggerAxis(Hand.kRight) > 0)
			control.camRotate = 1;
		else if(oi.xbox.getTriggerAxis(Hand.kLeft) > 0)
			control.camRotate = -1;
		else
			control.camRotate = 0;
		control.camTilt = oi.xbox.getX(Hand.kRight);

		return control;
	}
}