package org.usfirst.frc.team1807.robot;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends IterativeRobot {
    //Joysticks
    Joystick driveStickRight = new Joystick(0);
    Joystick driveStickLeft = new Joystick(1);
    Joystick joystickTodd =  new Joystick(2);
    //Chassis
    Talon chassisDriveRight = new Talon(0);
    Talon chassisDriveLeft = new Talon(2);



    //Collection
    Victor collectMotor = new Victor(2);
    DigitalInput collectSwitch = new DigitalInput(0);
    int collectState = 1;
    //Arm
    Talon largeArm = new Talon(3);
    Talon smallArm = new Talon(4);
    AnalogPotentiometer armPot = new AnalogPotentiometer(2);
    DigitalInput armSwitch = new DigitalInput(1);
    Encoder armEncoder = new Encoder(2, 3, true);
    int largeArmPos = 0;
    int smallArmPos = 0;
    int armEncDown;
    int armEncUp;
    Double armPotDown;
    Double armPotUp;
    //Elevator
    Talon elevatorMotor = new Talon(5);
    AnalogPotentiometer elevatorPot = new AnalogPotentiometer(3);
    int elevatorPos = 0;
    Double elevatorPotDown;
    Double elevatorPotUp;

    public void robotInit() {
    	//SmartDashboard.putNumber("Left", driveStickLeft.getRawAxis(1));
        //SmartDashboard.putNumber("Right", driveStickRight.getRawAxis(1));
        armPotDown = armPot.get();
        armPotUp = armPotDown + 0.02;
        armEncDown = 0;
        armEncUp = 200;
        elevatorPotUp = elevatorPot.get();
        elevatorPotDown = elevatorPotUp - 0;
    }

    public void autonomousInit() {

    }

    public void autonomousPeriodic() {
    	/*
    	if (elevatorPot.get() > elevatorPotDown) {
    		elevatorMotor.set(-0.5);
    	}
    	else {
    		elevatorMotor.set(0);
    	}
    	*/
    }

    /*** This function is called periodically during operator control*/
    public void teleopPeriodic() {
    	//SmartDashboard.putNumber("Encoder", armEncoder.get());
    	//Collection System
    	/*
        if (collectState == 1 && !joystickTodd.getRawButton(1)){
            collectMotor.set(0);
            collectState = 1;
        }
        else if (collectState == 1 && joystickTodd.getRawButton(1)){
            collectMotor.set(0.15);
            collectState = 2;
        }
        else if (collectState == 2 && !collectSwitch.get() && !joystickTodd.getRawButton(9)){
            collectMotor.set(0.15);
            collectState = 2;
        }
        else if (collectState == 2 && collectSwitch.get() && !joystickTodd.getRawButton(9)){
            collectMotor.set(0);
            collectState = 3;
        }
        else if (collectState == 2 && joystickTodd.getRawButton(9)){
            collectMotor.set(0);
            collectState = 1;
        }
        else if (collectState == 3 && joystickTodd.getRawButton(1)){
            collectMotor.set(0);
            collectState = 3;
        }
        else if (collectState == 3 && !joystickTodd.getRawButton(1)){
            collectMotor.set(0);
            collectState = 4;
        }
        else if (collectState == 4 && !joystickTodd.getRawButton(1)){
            collectMotor.set(0);
            collectState = 4;
        }
        else if (collectState == 4 && joystickTodd.getRawButton(1)){
            collectMotor.set(-0.15);
            collectState = 5;
        }
        else if (collectState == 5 && joystickTodd.getRawButton(1)){
            collectMotor.set(-0.15);
            collectState = 5;
        }
        else if (collectState == 5 && !joystickTodd.getRawButton(1)){
            collectMotor.set(0);
            collectState = 1;
        }

    	if(joystickTodd.getRawButton(4)){
    		collectMotor.set(1);
    	}
    	else if(joystickTodd.getRawButton(2)){
    		collectMotor.set(-1);
    	}
    	else{
    		collectMotor.set(0);
    	}
    	*/
    	//Chassis System



    	chassisDriveLeft.set(driveStickLeft.getRawAxis(1));
        chassisDriveRight.set(driveStickRight.getRawAxis(1));
        //Arm Mechanism
        //SmartDashboard.putNumber("pot.get()", armPot.get());

        if(joystickTodd.getRawButton(5)) {
            largeArmPos = 1;
        }else if(joystickTodd.getRawButton(3)) {
            largeArmPos = 2;
        }



        if(largeArmPos == 1){
            if(armPot.get() >= armPotUp) {
                largeArmPos = 0;
                largeArm.set(0);
            } else if (armPot.get() < armPotUp) {
                largeArm.set(.5);
            } else {
                largeArm.set(0);
                largeArmPos = 0;
            }
        } else if (largeArmPos == 2){
        	if(armPot.get() <= armPotDown){
                largeArmPos = 0;
                largeArm.set(0);
            }else if(armPot.get() > armPotDown){
                largeArm.set(-.2);
            } else {
                largeArm.set(0);
                largeArmPos = 0;
            }
        }


        if (joystickTodd.getRawButton(4)){
        	smallArmPos = 1;
        }
        else if (joystickTodd.getRawButton(2)){
        	smallArmPos = 2;
        }


        if (smallArmPos == 1){
        	if (armEncoder.get() >= armEncUp){
        		smallArm.set(0);
        		smallArmPos = 0;
        	}
        	else if (armEncoder.get() < armEncUp){
        		smallArm.set(-0.5);
        	}
        	else{
        		smallArm.set(0);
        		smallArmPos = 0;
        	}
        }
        else if (smallArmPos == 2){
        	if (armEncoder.get() <= armEncDown){
        		smallArm.set(0);
        		smallArmPos = 0;
        	}
        	else if (armEncoder.get() > armEncDown){
        		smallArm.set(0.5);
        	}
        	else{
        		smallArm.set(0);
        		smallArmPos = 0;
        	}
        }
        //Elevator
    	SmartDashboard.getNumber("pot.get()", elevatorPot.get());
    	if(joystickTodd.getRawButton(6)){
    		elevatorPos = 1;
    	}else if(joystickTodd.getRawButton(7)){
    		elevatorPos = 2;
    	}
    	if(elevatorPos == 1){
    		if(elevatorPot.get() >= elevatorPotUp) {
    			elevatorPos = 0;
    			elevatorMotor.set(0);
    		}else if(elevatorPot.get() < elevatorPotUp){
    			elevatorMotor.set(0.2);
    	    }else {
    	    	elevatorMotor.set(0);
    	    	elevatorPos = 0;
    	    }
    	}

    	else if(elevatorPos == 2){
    		if (elevatorPot.get() <= elevatorPotDown){
    			elevatorPos = 0;
    			elevatorMotor.set(0);
    		}else if(elevatorPot.get() > elevatorPotDown){
    			elevatorMotor.set(-0.2);
    		}else {
    			elevatorMotor.set(0);
    			elevatorPos = 0;
    		}
    	}

        //Dashboard
        //SmartDashboard.putNumber("Left", driveStickLeft.getRawAxis(1));
        //SmartDashboard.putNumber("Right", driveStickRight.getRawAxis(1));
    }

    /*** This function is called periodically during test mode*/
    public void testPeriodic() {

    }

    public void print(String key, boolean value){
        SmartDashboard.putBoolean(key, value);
    }
}