/**
* Copyright (c) 2015-2017 Luke Shumaker.
* Copyright (c) 2017 Cameron Richards.
* Copyright (c) 2017 Noah Gaeta.
* 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.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

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

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
            rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */
            lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */
            climber = new Talon(/*PWM*/3), /* PDP 4 */
            camRotate = new PIDServo(8),
            camTilt = new PIDServo(9);
        public final Encoder   lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);
        public final Encoder   rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);
        public final PIDSource lRate, rRate;
        public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
        public final DriverStation ds = DriverStation.getInstance();

        public DoubleSolenoid
                shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5),
                ged     = new DoubleSolenoid(/*PCM*/6, /*PCM*/7);
        
        private double maxRate = 0;
        private double minBatt = 20;

        public HwRobot() {
                lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
                rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);

                lDriveE.setPIDSourceType(PIDSourceType.kRate);
                rDriveE.setPIDSourceType(PIDSourceType.kRate);
                PDP.initTable(NetworkTable.getTable("PDP"));

                lRate = new RollingAvg(5, lDriveE);
                rRate = new RollingAvg(5, rDriveE);
        }

        public void run(Control c) {
                lDriveM.pidWrite(c.lDrive);
                rDriveM.pidWrite(-c.rDrive);
                climber.pidWrite(c.climber);
                camRotate.pidWrite(c.camRotate);
                camTilt.pidWrite(c.camTilt);

                shifter.set(c.highGear ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
                ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);

                double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2);
                SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
                SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate()));
                SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
                SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate()));
                SmartDashboard.putNumber("rate", rate);
                if (rate > maxRate)
                        maxRate = rate;
                SmartDashboard.putNumber("maxRate", maxRate);

                double batt = ds.getBatteryVoltage();
                SmartDashboard.putNumber("batt", batt);
                if (batt < minBatt)
                        minBatt = batt;
                SmartDashboard.putNumber("minBatt", minBatt);

                SmartDashboard.putBoolean("highGear", c.highGear);
                SmartDashboard.putBoolean("gedOut", c.gedOut);

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