package org.usfirst.frc.team4272.robot2016;

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

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.Talon;

import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;

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.
	 */

	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}

	private final PIDOutput
	    rDriveM1 = new Talon(/*PWM*/2), /* PDP 2, 3 */
	    rDriveM2 = new Talon(/*PWM*/4), /* PDP 4 */
	    lDriveM1 = new Talon(/*PWM*/1), /* PDP 12, 13 */
	    lDriveM2 = new Talon(/*PWM*/0), /* PDP 11 */
	    armM     = new Talon(/*PWM*/8), /* PDP 15 */
	    rfBallM  = new Talon(/*PWM*/3), /* PDP 0 */
	    lfBallM  = new Talon(/*PWM*/9), /* PDP 14 */
	    fBallM   = new PIDOutputSplitter(rfBallM, lfBallM),
	    bBallM   = new Talon(/*PWM*/6); /* PDP 10, 5 */

	public final Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true);
	public final DigitalInput armL = new DigitalInput(/*DIO*/3);
	public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
	public final Encoder   rDriveE = new Encoder(/*DIO*/5,/*DIO*/6);
	public final Encoder   lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */

	public HwRobot() {
		armE.setPIDSourceType(PIDSourceType.kDisplacement);
		lDriveE.setPIDSourceType(PIDSourceType.kRate);
		rDriveE.setPIDSourceType(PIDSourceType.kRate);
		PDP.initTable(NetworkTable.getTable("PDP"));
	}

	public void run(Control c) {
		lDriveM1.pidWrite( c.lDrive1);
		lDriveM2.pidWrite( c.lDrive2);
		rDriveM1.pidWrite(-c.rDrive1);
		rDriveM2.pidWrite(-c.rDrive2);
		armM.pidWrite(c.arm * 0.5);
		fBallM.pidWrite(-c.fBall);
		bBallM.pidWrite(-c.bBall);

		SmartDashboard.putNumber("fBall", c.fBall);
		SmartDashboard.putNumber("bBall", c.bBall);
		SmartDashboard.putNumber("armM", c.arm);

		SmartDashboard.putNumber("armE", armE.pidGet());
		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
		SmartDashboard.putNumber("lDriveE rate", lDriveE.getRate());
		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
		SmartDashboard.putNumber("rDriveE rate", rDriveE.getRate());
		SmartDashboard.putBoolean("ballL", ballL.get());
		SmartDashboard.putBoolean("armL", armL.get());

		PDP.updateTable();
		SmartDashboard.putData("PDP", PDP);
	}
}