package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Joystick;
import org.homestead.robotics.robot.DriveTrain;
import org.homestead.robotics.robot.Shooter;
import org.homestead.robotics.robot.Timer;

/**
 *
 * @author Driver
 */
public class DriveStation
{
    private static final int TOTAL_LINES = 6;
    private static String[] lines = new String[TOTAL_LINES];
    private Joystick rightDriveJoystick;
    private Joystick leftDriveJoystick;
    private Joystick shooterJoystick;
    private DriveTrain driveTrain;
    private Shooter shooter;

    public DriveStation()
    {
        rightDriveJoystick = new Joystick(1);
        leftDriveJoystick = new Joystick(2);
        shooterJoystick = new Joystick(3);

        driveTrain = new DriveTrain();
        shooter = new Shooter();
    }

    public void operatorControl()
    {
        handleJoystickButtons();
        driveTrain.drive(leftDriveJoystick.getX(), leftDriveJoystick.getY(), rightDriveJoystick.getX(), rightDriveJoystick.getY());
        updateLCD();
    }

    /**
     * Autonomous: Simply shoots
     */
    public void autonomousControl()
    {
        Timer fourfifthssecond = new Timer(0.8);
        Timer threesecond = new Timer(3);
        Timer fivesecond = new Timer(5);
        Timer fifteensecond = new Timer(15);
        fivesecond.set();
        fifteensecond.set();
        shooter.shoot();
        //shooter.voltageCompensation(getBatteryVoltage());
        while (!fifteensecond.isDone())//Team670Robot.isAuto())
        {
            if (fivesecond.isDone())
            {
                fourfifthssecond.set();
                fivesecond.unSet();
            }
            if (fourfifthssecond.isDone())
            {
                fivesecond.set();
                fourfifthssecond.unSet();
            }
            fifteensecond.step();
            fivesecond.step();
            fourfifthssecond.step();
            edu.wpi.first.wpilibj.Timer.delay(Robot2013.REFRESH_RATE);
        }

        shooter.stopShooter();
    }

    /**
     * Autonomous: Drives and shoots
     */
    /*
     * public void autonomousControl() { while(Team670Robot.isAuto()) {
     * shooter.shoot(); } shooter.stopShooter(); }
     */
    private void handleJoystickButtons()
    {
        driveTrain.togglePrecisionMode(((leftDriveJoystick.getRawButton(2) || rightDriveJoystick.getRawButton(2)) ? true : false));
        driveTrain.toggleBoostMode(((leftDriveJoystick.getTrigger() || rightDriveJoystick.getTrigger()) ? true : false));
        if (rightDriveJoystick.getRawButton(7))
        {
            driveTrain.toggleDriveMode();
        }
        driveTrain.stepTimer();

        if (shooterJoystick.getRawButton(3))
        {
            shooter.voltageCompensation();
        }
        setLine(1, "Shooter throttle: " + shooter.getThrottle());
        
        if (shooterJoystick.getRawButton(4))
        {
            shooter.setForward();
            DriveStation.setLine(2, "Button 4, setForward()");
        }
        if (shooterJoystick.getRawButton(5))
        {
            shooter.setReverse();
            DriveStation.setLine(2, "Button 5, setReverse()");            
        }
        if (shooterJoystick.getRawButton(2))
        {
            shooter.setOff();
            DriveStation.setLine(2, "Button 2, setOff()");            
        }

        if (shooterJoystick.getTrigger())
        {
            shooter.shoot();
        }
        else
        {
            shooter.stopShooter();
        }

        if (shooterJoystick.getRawButton(10))
        {
            shooter.throttleUp();
        }
        else if (shooterJoystick.getRawButton(11))
        {
            shooter.throttleDown();
        }
    }

    public static void setLine(int key, String value)
    {
        if (key > TOTAL_LINES || key < 1)
        {
            return;
        }
        lines[key - 1] = value;
    }

    public void updateLCD()
    {
        for (int i = 0; i < lines.length; i++)
        {
            if (lines[i] == null)
            {
                lines[i] = "";
            }
        }

        DriverStationLCD stationLCD = DriverStationLCD.getInstance();

        stationLCD.println(DriverStationLCD.Line.kMain6, 1, "                         ");
        stationLCD.println(DriverStationLCD.Line.kUser2, 1, "                         ");
        stationLCD.println(DriverStationLCD.Line.kUser3, 1, "                         ");
        stationLCD.println(DriverStationLCD.Line.kUser4, 1, "                         ");
        stationLCD.println(DriverStationLCD.Line.kUser5, 1, "                         ");
        stationLCD.println(DriverStationLCD.Line.kUser6, 1, "                         ");

        stationLCD.println(DriverStationLCD.Line.kMain6, 1, lines[0]);
        stationLCD.println(DriverStationLCD.Line.kUser2, 1, lines[1]);
        stationLCD.println(DriverStationLCD.Line.kUser3, 1, lines[2]);
        stationLCD.println(DriverStationLCD.Line.kUser4, 1, lines[3]);
        stationLCD.println(DriverStationLCD.Line.kUser5, 1, lines[4]);
        stationLCD.println(DriverStationLCD.Line.kUser6, 1, lines[5]);

        stationLCD.updateLCD();
    }
}