/**
 * 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;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;

public class Autonomous {
	private static interface Command {
		/** Mutate the @{code Control} (possible because it is
		 * passed by reference), and return whether or not
		 * this command has finished yet.
		 *
		 * @param c The structure to control the robot
		 * @return Whether the command has finished.
		 */
		public boolean execute(Control c);
	}

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

		private final HwRobot robot;
		private final double pctThrottle;
		private final double lSpeed, lDistTarget;
		private final double rSpeed, rDistTarget;
		private double lMin = 0, rMin = 0;
		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;

			/* Left side */
			c.lDrive = lSpeed;
			if (Math.abs(1-lPct) < pctTaper) {
				/* Taper off, slowing down as we approach the designed target */
				c.lDrive *= Math.abs((1-lPct)/pctTaper);
				if (Math.abs(robot.lRate.pidGet()) < 0.5) {
					/* oops, tapered off too quickly */
					lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05; /* Multiplier */
				}
				if (c.lDrive > lSpeed) { /* Speed too high */
					c.lDrive = lSpeed;
				}
			}
			c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct);

			/* Right side */
			c.rDrive = rSpeed;
			if (Math.abs(1-rPct) < pctTaper) {
				/* Taper off, slowing down as we approach the designated target */
				c.rDrive *= Math.abs((1-rPct)/pctTaper);
				if (Math.abs(robot.rRate.pidGet()) < 0.5) {
					/* oops, tapered off too quickly */
					rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05; /* Multiplier */
				}
				if (c.rDrive > rSpeed) { /* Speed too high */
					c.rDrive = rSpeed;
				}
			}
			c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);

			/* Make sure the left and right sides are even */
			if (Math.abs(lPct - rPct) > pctTolerance) {
				if (lPct > rPct) {
					c.lDrive *= pctThrottle;
				} else {
					c.rDrive *= pctThrottle;
				}
			}

			SmartDashboard.putNumber("lPct", lPct);
			SmartDashboard.putNumber("rPct", rPct);
			SmartDashboard.putNumber("lMin", lMin);
			SmartDashboard.putNumber("rMin", rMin);
			SmartDashboard.putNumber("lDrive", c.lDrive);
			SmartDashboard.putNumber("rDrive", c.rDrive);
			return lPct >= 1 && rPct >= 1; 
		}
	}
	private static class Stop implements Command {
		public boolean execute(Control c) {
			c.lDrive = c.rDrive = 0;
			return true;
		}
	}

	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 static Command stop(HwRobot robot) {
		return new Stop();
	}

	private final HwRobot robot;
	private final Command[] commands;
	private int step = 0;
	private static final SendableChooser modeChooser = new SendableChooser();

	public static void robotInit() {
		modeChooser.addObject("Center Peg", "Center Peg");
		modeChooser.addObject("Left Peg", "Left Peg");
		modeChooser.addObject("Right Peg", "Right Peg");
		SmartDashboard.putData("Autonomous Mode", modeChooser);
	}

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

		String mode = (String) modeChooser.getSelected();

		switch (mode) {
		case "Center Peg":
			commands = new Command[]{
				driveDistance(robot, 0.4, 8.75),
				stop(robot),
			};
			break;
		case "Left Peg":
			commands = new Command[]{
				driveDistance(robot, 0.4, 3.5),
				turnDegrees(robot, 0.4, 60),
				driveDistance(robot, 0.4, 8.3),
				stop(robot),
			};
			break;
		case "Right Peg":
			commands = new Command[]{
				driveDistance(robot, 0.4, 3.5),
				turnDegrees(robot, 0.4, -60),
				driveDistance(robot, 0.4, 8.3),
				stop(robot),
			};
			break;
		default:
			commands = new Command[]{stop(robot)};
		}
	}

	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;
	}
}