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

public class Autonomous {
	/*=[ Overview ]=======================================================*\
	||                                                                    ||
	|| Most of the robot's code is very simple.  Autonomous is less       ||
	|| simple (it accounts for almost half of the total code!).           ||
	||                                                                    ||
	|| But we've organized things to make working on it fairly simple.    ||
	||                                                                    ||
	|| The exterior interface is simple:                                  ||
	||  1. call `Autonomous.robotInit()` once when the robot turns on;    ||
	||  2. call `a = new Autonomous(robot)` whenever autonomous mode      ||
	||     starts;                                                        ||
	||  3. call `control = a.run(control)` every 20ms to ask it what it   ||
	||     wants the hardware to do.                                      ||
	||                                                                    ||
	|| But it's hard to write moderately complex autonomous modes         ||
	|| directly with that.  It's hard to keep track of multiple           ||
	|| steps.  So, internally, we invent "Commands", and say that an      ||
	|| autonomous "Mode" is a function that returns a list of commands.   ||
	||                                                                    ||
	|| This file is divides in to 3 sections:                             ||
	||  1. Modes: Define the autonomous modes here.                       ||
	||  2. Abstraction: Implement the abstraction described above; define ||
	||     what a "Command" is and what a "Mode" is, and simple code to   ||
	||     step through the mode's commands as they run.		      ||
	||  3. Commands: Definitions of complex commands (and functions to    ||
	||     generate commands).                                            ||
	||                                                                    ||
	\*====================================================================*/

	/*====================================================================*\
	|| Modes: Define the high-level autonomous modes                      ||
	\*====================================================================*/

	public static void networkInit() {
		Command init = c->{
			c.lDrive = c.rDrive = 0;
			c.highGear = c.gedOut = false;
			return true;
		};

		Command stop = c->{c.lDrive = c.rDrive = 0; return true;};

		modeChooser.addObject("Drive 10 feet", robot->new Command[]{
				init,
				driveDistance(robot, 0.4, 10),
				stop,
			});
		
		modeChooser.addObject("Drive 7 seconds", robot->new Command[]{
				init,
				timeout(7.0, c->{c.lDrive = c.rDrive = 0.4; return false;}),
				stop,
			});
		modeChooser.addObject("Center Peg", robot->new Command[]{
				init,
				timeout(6.0, driveDistance(robot, 0.4, 8.75)),
				stop,
				c->{c.gedOut = true; return true;},
				driveDistance(robot, 0.4, -3),
				stop,
			});
		modeChooser.addObject("Left Peg", robot->new Command[]{
				init,
				driveDistance(robot, 0.4, 8.4),
				turnDegrees(robot, 0.4, 70),
				driveDistance(robot, 0.4, 2.16),
				timeout(1.0, c->false),
				c->{c.gedOut = true; return true;},
				timeout(1.0, c->false),
				driveDistance(robot, 0.4, -3),
				c->{c.gedOut = false; return true;},
				turnDegrees(robot, 0.4, -70),
				driveDistance(robot, 1.0, 15),
				stop,
			});
		modeChooser.addObject("Right Peg", robot->new Command[]{
				init,
				driveDistance(robot, 0.4, 8.4),
				turnDegrees(robot, 0.4, -70),
				driveDistance(robot, 0.4, 2.16),
				timeout(1.0, c->false),
				c->{c.gedOut = true; return true;},
				timeout(1.0, c->false),
				driveDistance(robot, 0.4, -3),
				c->{c.gedOut = false; return true;},
				turnDegrees(robot, 0.4, 70),
				driveDistance(robot, 1.0, 15),
				stop,
			});
		modeChooser.addObject("Stand Still", robot->new Command[]{
				stop,
			});

		SmartDashboard.putData("Autonomous Mode", modeChooser);
	}

	/*====================================================================*\
	|| Abstraction: Run whichever mode is selected.                       ||
	\*====================================================================*/

	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 interface Mode {
		/**
		 * Given a some robot hardware, instantiate a sequence
		 * of @{code Command}s to run during Autonomous.
		 *
		 * @param robot A handle on the robot hardware.
		 * @return The commands to run during Autonomous.
		 */
		public Command[] getCommands(HwRobot robot);
	}

	private static final SendableChooser<Mode> modeChooser = new SendableChooser<Mode>();

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

	public Autonomous(HwRobot robot) {
		commands = modeChooser.getSelected().getCommands(robot);
	}

	public Control run(Control c) {
		if (step < commands.length) {
			if (commands[step].execute(c))
				step++;
		}
		SmartDashboard.putNumber("step", step);
		return c;
	}

	/*====================================================================*\
	|| Commands: For use in modes (above)                                 ||
	\*====================================================================*/

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

		private double
			cntT = 0,
			cntL = 0,
			cntR = 0;

		private final HwRobot robot;
		private final double pctThrottle;
		private final double pctTolerance;
		private final double lSpeed, lDistTarget;
		private final double rSpeed, rDistTarget;
		private double lMin = 0, rMin = 0;
		public DriveDistance(HwRobot robot, double pctThrottle, double pctTolerance,
		                     double lSpeed, double lDistTarget,
		                     double rSpeed, double rDistTarget) {
			this.robot = robot;
			this.pctThrottle = pctThrottle;
			this.pctTolerance = pctTolerance;
			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);

			cntT++;
			double throttle = Math.min(Math.abs(lPct - rPct), pctTolerance)/pctTolerance;
			if (lPct > rPct) {
				c.lDrive *= 1.0-(pctThrottle*throttle);
				//c.rDrive /= 1.0-(pctThrottle*throttle);
				cntL += throttle;
			} else {
				//c.lDrive /= 1.0-(pctThrottle*throttle);
				c.rDrive *= 1.0-(pctThrottle*throttle);
				cntR += throttle;
			}

			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);
			SmartDashboard.putNumber("lCnt", cntL/cntT);
			SmartDashboard.putNumber("rCnt", cntR/cntT);
			return lPct >= 1 && rPct >= 1;
		}
	}
	private static class TimedCommand implements Command {
		private final Command inner;
		private final double secs;
		private final Timer t = new Timer();
		private boolean initialized = false;
		public TimedCommand(Command inner, double secs) {
			this.inner = inner;
			this.secs = secs;
		}
		public boolean execute(Control c) {
			if (!initialized) {
				t.reset();
				t.start();
				initialized = true;
			}
			if (t.get() < secs) {
				return inner.execute(c);
			} else {
				return true;
			}
		}
	}

	private static Command driveDistance(HwRobot robot, double speed, double dist) {
		return new DriveDistance(robot, 1.0, (1/36.0)/dist,
		                         speed, dist,
		                         speed, dist);
	}
	private static Command turnRadians(HwRobot robot, double speed, double rad) {
		double dist = rad*robot.axleWidth/2;
		return new DriveDistance(robot, 0.33, (1/36.0)/dist,
		                         speed, Math.copySign(dist, rad),
		                         speed, Math.copySign(dist, -rad));
	}
	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
		return turnRadians(robot, speed, deg * Math.PI/180);
	}
	private static Command timeout(double secs, Command inner) {
		return new TimedCommand(inner, secs);
	}
}