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

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

public class Teleop {
        private final HwRobot robot;
        private final Timer timer = new Timer();
        private final Preferences prefs = Preferences.getInstance();

        private double lastTime;
        private double currentTime;
        private boolean state = false;

        public Teleop(HwRobot robot) {
                this.robot = robot;
                timer.start();
        }

        private static double jsScale(Joystick j) {
                double y = j.getY();/* +:forward; -:backward */
                double z = j.getZ();/* +:more-sensitive; -:less-sensitive */
                return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
        }

        private static double bound(double min, double v, double max) {
                return Math.min(Math.max(v, min), max);
        }

        private static double oneIf(boolean b) {
                return b ? 1 : 0;
        }
        
        public Control run(Control control, HwOI oi) {
                /* drive */
                control.lDrive = -jsScale(oi.lStick);
                control.rDrive = -jsScale(oi.rStick);

                /* shifting */
                double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28);
                double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28);
                if (oi.rStick.getTrigger()) {
                        control.highGear = true;
                } else if (oi.lStick.getTrigger()) {
                        control.highGear = false;
                } else {
                        double speed = Math.abs((robot.lRate.pidGet() + robot.rRate.pidGet()) / 2);
                        if (!control.highGear) {
                                if (speed > shiftUp)
                                        control.highGear = true;
                        } else {
                                if (speed < shiftDown)
                                        control.highGear = false;
                        }
                }

                /* auto gear wiggle */
                if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) {
                        if (timer.get() > 0.30) {
                                timer.reset();
                        }
                        if (timer.get() < 0.15) {
                                control.lDrive = 0.5;
                                control.rDrive = 0;
                        } else {
                                control.lDrive = 0;
                                control.rDrive = 0.5;
                        }
                }

                /* climber */
                control.climber = oi.xbox.getY(Hand.kLeft);
                if (control.climber <= 0.2) {
                        control.climber = 0;
                }

                /* GED */
                control.gedOut = oi.xbox.getTriggerAxis(Hand.kRight) > 0.5;

                /* camera */
                // AnnaLaura wants the GED on the trigger, so we'll
                // have to find somewhere else for the camera.
                /*
                if (oi.xbox.getTriggerAxis(Hand.kRight) > 0) {
                        control.camRotate = 1;
                } else if (oi.xbox.getTriggerAxis(Hand.kLeft) > 0) {
                        control.camRotate = -1;
                } else {
                        control.camRotate = 0;
                }
                control.camTilt = oi.xbox.getX(Hand.kRight);
                */
                return control;
        }
}