package org.usfirst.frc.team4828;

import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Timer;

public class Robot extends IterativeRobot {
    private Joystick driveStick;
    private DriveTrain drive;
    private AHRS navx;
    private DigitalInput ir;
    private UltraThread us;
    private DigitalInput[] dipSwitch;
    private int autonSelect;

    @Override
    public void robotInit() {
        super.robotInit();
        System.out.println("THE ROBOT TURNED ON");
        driveStick = new Joystick(0);
        drive = new DriveTrain(1, 2, 3, 4);
        navx = new AHRS(SPI.Port.kMXP);
        ir = new DigitalInput(2);
        us = new UltraThread(0);
        dipSwitch = new DigitalInput[4];
        dipSwitch[0] = new DigitalInput(0);
        dipSwitch[1] = new DigitalInput(1);
        dipSwitch[2] = new DigitalInput(2);
        dipSwitch[3] = new DigitalInput(3);
    }

    @Override
    public void autonomousInit() {
        super.autonomousInit();
        System.out.println("Entering auton...");
        for (int i = 0; i < 4; i++) {
            autonSelect += (dipSwitch[i].get() ? 1 : 0) * (1 << i);
        }
        switch (autonSelect) {
            case 0:
                // Shoot 10 fuel
                break;
            case 1:
                // Place gear on right side
                break;
            case 2:
                // Place gear on center
                break;
            case 3:
                // Place gear on center
                break;
            case 4:
                // Shoot 10 fuel and place gear on left side
                break;
            case 5:
                // The crazy running into hopper and shooting tons of balls plan
                break;
            default:
                // Do nothing
        }
    }

    @Override
    public void autonomousPeriodic() {
        Timer.delay(.1);
        super.autonomousPeriodic();
    }

    @Override
    public void teleopInit() {
        super.teleopInit();
        System.out.println("Entering teleop...");
    }

    @Override
    public void teleopPeriodic() {
        super.teleopPeriodic();
        drive.mecanumDrive(driveStick.getX(), driveStick.getY(), driveStick.getTwist(), navx.getAngle());
        if (driveStick.getRawButton(11)) {
            navx.reset();
        }

        //System.out.println("Angle: " + navx.getAngle());
        //System.out.println("IR Status: " + ir.get());
    }

    @Override
    public void testInit() {
        super.testInit();
        System.out.println("Entering test...");
        us.start();
    }

    @Override
    public void testPeriodic() {

        System.out.println("Ultrasonic Dist: " + us.distIn + " inches");
        Timer.delay(0.1);
    }

    @Override
    public void disabledInit() {
        us.terminate();
        System.out.println("Stopping thread");
    }
}

@@ -909,17 +909,17 @@
 alInput(
-0
+6
 );%0A     
@@ -953,17 +953,17 @@
 alInput(
-1
+4
 );%0A     
@@ -989,33 +989,33 @@
 ew DigitalInput(
-2
+5
 );%0A        dipSw
@@ -1137,57 +1137,8 @@
 ();%0A
-        System.out.println(%22Entering auton...%22);%0A
     
@@ -1239,16 +1239,16 @@
  %3C%3C i);%0A
-
         
@@ -1245,24 +1245,92 @@
 ;%0A        %7D%0A
+        System.out.println(%22Entering auton number %22 + autonSelect);%0A
         swit
@@ -2738,18 +2738,18 @@
         
-us
+//
 .start()
@@ -2804,25 +2804,24 @@
 dic() %7B%0A
-%0A
         
 System.o
@@ -2804,32 +2804,168 @@
 dic() %7B%0A        
+for (int i = 0; i %3C 4; i++) %7B%0A            System.out.print(%22 %22 + dipSwitch%5Bi%5D.get());%0A        %7D%0A        System.out.println();%0A        //
 System.out.print

Put ports in order