/**
 * Copyright (c) 2017 Luke Shumaker.
 * Copyright (c) 2017 Thomas Griffith.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the FIRST nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package org.usfirst.frc.team4272.robot2017;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Autonomous {
	private static interface Command {
		public boolean execute(Control c);
	}

	private static class DriveDistance implements Command {
		private static final double pctTaper = 1.0/3;
		private static final double pctTolerance = 0.005;
		private static final double distFinished = 0.1;

		private final HwRobot robot;
		private final double pctThrottle;
		private final double lSpeed, lDistTarget;
		private final double rSpeed, rDistTarget;
		public DriveDistance(HwRobot robot, double pctThrottle,
		                     double lSpeed, double lDistTarget,
		                     double rSpeed, double rDistTarget) {
			this.robot = robot;
			this.pctThrottle = pctThrottle;
			this.lSpeed = lSpeed;
			this.lDistTarget = lDistTarget;
			this.rSpeed = rSpeed;
			this.rDistTarget = rDistTarget;
		}
		private boolean initialized = false;
		public boolean execute(Control c) {
			if (!initialized) {
				robot.lDriveE.reset();
				robot.rDriveE.reset();
				initialized = true;
			}
			double lDistCurr = robot.lDriveE.getDistance();
			double rDistCurr = robot.rDriveE.getDistance();
			double lPct = lDistCurr/lDistTarget;
			double rPct = rDistCurr/rDistTarget;

			c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct);
			if (Math.abs(1-lPct) < pctTaper) {
				c.lDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper));
			}

			c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct);
			if (Math.abs(1-rPct) < pctTaper) {
				c.rDrive *= Math.sqrt(Math.abs((1-rPct)/pctTaper));
			}

			if (Math.abs(lPct - rPct) > pctTolerance) {
				if (lPct > rPct) {
					c.lDrive *= pctThrottle;
				} else {
					c.rDrive *= pctThrottle;
				}
			}
			return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished;
		}
	}

	private static final double axleWidth = 2; // in feet
	private static Command driveDistance(HwRobot robot, double speed, double dist) {
		return new DriveDistance(robot, 0.2,
		                         speed, dist,
		                         speed, dist);
	}
	private static Command turnRadians(HwRobot robot, double speed, double rad) {
		return new DriveDistance(robot, 0.6,
		                         speed, Math.copySign(rad*axleWidth/2, rad),
		                         speed, Math.copySign(rad*axleWidth/2, -rad));
	}
	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
		return turnRadians(robot, speed, deg * Math.PI/180);
	}

	private final HwRobot robot;
	private final Command[] commands;
	private int step = 0;

	public Autonomous(HwRobot robot) {
		this.robot = robot;

		// Center peg
		/*
		commands = new Command[]{
			driveDistance(robot, 0.3, 8.75),
		};
		*/

		// Left peg
		commands = new Command[]{
			driveDistance(robot, 0.3, 3.5),
			turnDegrees(robot, 0.3, 60),
			driveDistance(robot, 0.3, 8.3),
		};
	}

	public Control run(Control c) {
		c.gedOut = false;
		c.highGear = false;

		if (step < commands.length) {
			if (commands[step].execute(c))
				step++;
		}

		SmartDashboard.putNumber("step", step);
		return c;
	}
}