package org.usfirst.frc.team4272.robot2015;

import org.mckenzierobotics.lib.robot.PIDOutputSplitter;
import org.usfirst.frc.team4272.robotlib.DoubleSolenoid;
import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Talon;

public class HwRobot {
	/* Relay == a Spike */
	/* PCM = Pneumatics Control Module */

	/* All of the numbered inputs are in the classes:
	 * - DIO: 0-9
	 * - Relay: 0-3
	 * - Analog In: 0-3
	 * - PWM: 0-9
	 * - PCM: 0-7 (the PCM is connected via CAN).
	 * - CAN
	 * 
	 * For completeness, the roboRIO also has: i2c, RS-232, SPI,
	 * RSL, 2xUSB-A, an accelerometer, and an expansion port.
	 *
	 * And, for communication: USB-B and Ethernet.
	 */
	
	static class OutDumper implements PIDOutput {
		private String p;
		public OutDumper(String prefix) { p = prefix; }
		public void pidWrite(double v) {
			System.out.println(p+": "+v);
		}
	}
	static class InDumper implements PIDSource {
		private String p;
		public final PIDSource r;
		public InDumper(String prefix, PIDSource real) { p = prefix; r = real; }
		public double pidGet() {
			double v = r.pidGet();
			System.out.println(p+": "+v);
			return v;
		}
	}
	public Encoder   lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
	public PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0),
	                                                 new Talon(/*PWM*/1));
	public Encoder   rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
	public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2),
	                                                 new Talon(/*PWM*/3));
	public Encoder   winchE = new Encoder(/*DIO*/4,/*DIO*/5);//),
	public static DigitalInput winchT = new DigitalInput(/*DIO*/6);
	public static DigitalInput winchB = new DigitalInput(/*DIO*/7);
	public PIDOutput winchM = new LimitSwitchedPIDOutput(
		new PIDOutputSplitter(new Talon(/*PWM*/4),
		                      new Talon(/*PWM*/5)),
		winchT, false, winchB, false);
	public DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3);
	public DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1);
	public Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth);
	public Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth);

	public HwRobot() {
		lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
		rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
		winchE.setPIDSourceParameter(PIDSourceParameter.kRate);
	}
}