package org.usfirst.frc.team4085.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

import org.usfirst.frc.team4085.robot.commands.ExampleCommand;
import org.usfirst.frc.team4085.robot.subsystems.ExampleSubsystem;

import edu.wpi.first.wpilibj.RobotDrive.MotorType;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.smartdashboard.*;
import edu.wpi.first.wpilibj.Timer;




/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {

	public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
	public static OI oi;

	CANTalon dmotor1 = new CANTalon(1);//Front Left Drive Motor
    CANTalon dmotor2 = new CANTalon(2);//Front Right Drive Motor
    CANTalon dmotor3 = new CANTalon(3);//Back Left Drive Motor
    CANTalon dmotor4 = new CANTalon(4);//Back Right Drive Motor
    CANTalon wmotor2 = new CANTalon(5);//Left Wheel Arm
    CANTalon wmotor1 = new CANTalon(6);//Right Wheel Arm
    CANTalon lmotor1 = new CANTalon(7);//Winch Motor

    Solenoid solenoid1 = new Solenoid(1); // Left Wheel Arm Solenoid
    Solenoid solenoid2 = new Solenoid(2); // Right Wheel Arm Solenoid
    Solenoid solenoid3 = new Solenoid(3); // Fork Arm Solenoid
    Compressor comp = new Compressor();



    RobotDrive rdrive = new RobotDrive(dmotor1,dmotor2,dmotor3,dmotor4);//Defines The Location of The Drive Motors

    Joystick Xbox = new Joystick(0);// Driver Controls
    Joystick lStick = new Joystick(1);//Left Wheel Arm Set Control
    Joystick rStick = new Joystick(2);//Right Wheel Arm Set Control

    Command autonomousCommand;

    AnalogInput sonar = new AnalogInput(0);// Sonar Sensor
    SmartDashboard smart = new SmartDashboard();

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
		oi = new OI();
        // instantiate the command used for the autonomous period
        autonomousCommand = new ExampleCommand();

        //Inverts Right Drive Motors, Needed For Correct Mecanum Code
        rdrive.setInvertedMotor(MotorType.kFrontRight, true);
        rdrive.setInvertedMotor(MotorType.kRearRight, true);
        rdrive.setInvertedMotor(MotorType.kFrontLeft, false);
        rdrive.setInvertedMotor(MotorType.kRearLeft, false);

   /*     CameraServer.getInstance().setQuality(1/1000);

        CameraServer.getInstance().startAutomaticCapture("cam0");
    */
    }

	public void disabledPeriodic() {
		Scheduler.getInstance().run();
	}

    public void autonomousInit() {
        // schedule the autonomous command (example)
        if (autonomousCommand != null) autonomousCommand.start();
        dmotor1.setPosition(0);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
        /*smart.putString("Encoder Position", "" + dmotor1.getEncPosition());

        dmotor1.setPosition(0);


        double P;
        P = dmotor1.getEncPosition();
        */

        // Opens All of The Arms
        solenoid1.set(true);
        //Timer.delay(1);
        solenoid2.set(true);
        //Timer.delay(1);
        solenoid3.set(true);
        Timer.delay(.5);

        //Drive Forward
        rdrive.mecanumDrive_Polar(-1, 0, 0);
        Timer.delay(.5);
        rdrive.mecanumDrive_Polar(-1, 0, 0);
        Timer.delay(.5);

        //if(P <= -475){
        	//Stop, and Pick up
        	rdrive.mecanumDrive_Polar(0,0,0);
        	Timer.delay(.5);
        	solenoid1.set(false);
        	//Timer.delay(1);
        	solenoid2.set(false);
        	//Timer.delay(1);

        	solenoid3.set(false);
        	Timer.delay(.5);

        	dmotor1.setPosition(0);




           // }
        //Drive Backward
        rdrive.mecanumDrive_Polar(1, 0, 0);
        Timer.delay(.5);
        rdrive.mecanumDrive_Polar(1, 0, 0);
        Timer.delay(.5);
        rdrive.mecanumDrive_Polar(1, 0, 0);
        Timer.delay(.5);
        rdrive.mecanumDrive_Polar(1, 0, 0);
        Timer.delay(.5);




        //if(P > 800){
        	//Rotate and Drop
        	rdrive.mecanumDrive_Polar(0, 0, 1);//2 = 60degrees
        	Timer.delay(.5);
        	rdrive.mecanumDrive_Polar(0, 0, 1);
        	Timer.delay(.5);
        	rdrive.mecanumDrive_Polar(0, 0, 1);
        	Timer.delay(.5);
        	rdrive.mecanumDrive_Polar(0, 0, 1);
        	Timer.delay(.5);
        	rdrive.mecanumDrive_Polar(0, 0, 0);
        	Timer.delay(.5);
        	solenoid1.set(true);
        	//Timer.delay(1);
        	solenoid2.set(true);
        	//Timer.delay(1);
        	solenoid3.set(true);
        	Timer.delay(.5);

        	//Back off the Tote
        	rdrive.mecanumDrive_Polar(1, 0, 0);
        	Timer.delay(.5);
        	rdrive.mecanumDrive_Polar(1, 0, 0);
        	Timer.delay(.5);
        	rdrive.mecanumDrive_Polar(0, 0, 0);
        	Timer.delay(.5);

        //}



        }






    public void teleopInit() {
		// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to
        // continue until interrupted by another command, remove
        // this line or comment it out.
        if (autonomousCommand != null) autonomousCommand.cancel();



    }

    /**
     * This function is called when the disabled button is hit.
     * You can use it to reset subsystems before shutting down.
     */
    public void disabledInit(){

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {

    		Scheduler.getInstance().run();

        //Compressor
        comp.start();

        //Solenoid Control
        double X1, X2;

        X1 = lStick.getX();
        X2 = rStick.getX();


        //Left Arm
        if(X1 > .5){//Open
        	solenoid1.set(false);
        }


        if(X1 < -.5){//Close
        	solenoid1.set(true);
        }


        //Right Arm
        if(X2 > .5){//Close
        	solenoid2.set(true);
        }


        if(X2 < -.5){//Open
        	solenoid2.set(false);
        }


        //Fork Arms
        if(lStick.getRawButton(1)){//Close
        	solenoid3.set(true);
        }

        if(rStick.getRawButton(1)){//Open
        	solenoid3.set(false);
        }



        //Lift Control
        if(lStick.getRawButton(3)){//Lift Up
        	lmotor1.set(1);
        }

        if(lStick.getRawButton(2)){//Lift Down
        	lmotor1.set(-1);
        }

        if(rStick.getRawButton(3)){//Stop Lift
        	lmotor1.set(0);
        }


        //Wheel Motor Control
        double Y1, Y2;

        Y1 = lStick.getY();
        Y2 = rStick.getY();

        wmotor1.set(Y2*-1);
        wmotor2.set(Y1*-1);

        //Drive Control
        double X,Y,Z;

        X = Xbox.getRawAxis(0);
        Y = Xbox.getRawAxis(1);
        Z = Xbox.getRawAxis(4);

        //Left Stick Deadzone

        double mag = Math.sqrt((X*X)+(Y*Y));

        if(Math.abs(mag) < .3){
        	mag = 0;
        }

        //Right Stick Deadzone
        if(Math.abs(Z) < .3){
        	Z = 0;
        }



        //Mecanum Drive Statement
        rdrive.mecanumDrive_Polar(mag*-1 , Xbox.getDirectionDegrees(), Z);
    }

    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }

}