/*

THIS CODE IS MADE BY THE BEST PEOPLE IN THE WORLD, DO NOT STEAL.

THINGS TO  ARE IN CAPS

Instructions are in sentence form.

*/
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.*;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboardData; //WORKING ON THIS(1)

public class Robot4334 extends SimpleRobot {
                                                   //DECLARE ALL METHODS IN INIT AND SIMPLEROBOT
    public RobotDrive drivetrain;
    public Joystick joystick;
    public Joystick XBoxController;
    public Gyro gyro; //gyro
    public Timer timer;
    public Encoder encoder;
    public static final double DRIVE_STRAIGHT = 0;
    public static final double DRIVE_FULL_SPEED = 1; //Variable set for driving full speed for ease of use
    public static final int LeftX = 1;
    public static final int LeftY = 2;
    public static final int Triggers = 3; //(Each trigger = 0 to 1, axis value = right - left)
    public static final int RightX = 4;
    public static final int RightY = 5;
    public static final int DPad = 6;
    public static String log(String theMessage){ // Logger -System.out.println function
        System.out.println(theMessage);
        return theMessage;
    }
    public void BRAKE(){ //Brake method - stops motors
        drivetrain.drive(0.0,0.0);
        drivetrain.stopMotor();
    }
    public void EMERGENCY(){ //ADD ALL FUNCTIONS HERE TO STOP ON ROBOT DURING EMERGENCY
        drivetrain.drive(0.0,0.0);
            Timer.delay(10.0);
        drivetrain.stopMotor();
    }
    public void RESET(){
        gyro.reset();
        timer.reset();
        drivetrain.stopMotor();
    }
    public double returnSpeed(){
            double speed = XBoxController.getRawAxis(LeftY);
            return speed;
    }
        public double returnTurn(){
            double Turn = XBoxController.getRawAxis(LeftX);
            return Turn;
    }

/*
XBoxController index Axis: (.getRawAxis())
* Set XBoxController settings in driver (.getRawButton())
*/
    public void robotInit(){ //REMEMBER TO INITIALIZE EVERYTHING
        drivetrain = new RobotDrive(1, 2, 3, 4);
        joystick = new Joystick(1);
        XBoxController = new Joystick(1);
        gyro = new Gyro(1); // FIND CORRECT INPUT FOR THIS
   //   encoder = new Encoder(); //FIND CORRECT INPUT
    }

    public void autonomous(){
        while(isAutonomous()&&isEnabled()){ //Makes sure in autonomous.
            drivetrain.drive(.5,.0);
            Timer.delay(1);
            drivetrain.drive(.1,.5);
            Timer.delay(1);
        }
    }

    public void operatorControl() {
        while (isOperatorControl() && isEnabled()){ //Make sure in driver
            log("\n\n\n\n\n\n\n\n\n\n\nIn operator control");
            if(XBoxController.getRawButton(2)){BRAKE();}else{
                double speed =returnSpeed();
                double turn =returnTurn();
                drivetrain.drive(speed,turn);
            }
        }
    }
}