// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

public class Robot extends TimedRobot {
  private Command m_autonomousCommand;

  private RobotContainer m_robotContainer;

  @Override
  public void robotInit() {
    m_robotContainer = new RobotContainer();
  }

  @Override
  public void robotPeriodic() {
    CommandScheduler.getInstance().run();
  }

  @Override
  public void disabledInit() {}

  @Override
  public void disabledPeriodic() {}

  @Override
  public void disabledExit() {}

  @Override
  public void autonomousInit() {
    m_autonomousCommand = m_robotContainer.getAutonomousCommand();

    if (m_autonomousCommand != null) {
      m_autonomousCommand.schedule();
    }
  }

  @Override
  public void autonomousPeriodic() {}

  @Override
  public void autonomousExit() {}

  @Override
  public void teleopInit() {
    if (m_autonomousCommand != null) {
      m_autonomousCommand.cancel();
    }
  }

  // GenericHID controller = Constants.controller;
  // CANSparkMax shooter_top = new CANSparkMax(21, MotorType.kBrushless);
  // CANSparkMax shooter_bottom = new CANSparkMax(22, MotorType.kBrushless);
  // CANSparkMax intakeL = new CANSparkMax(31, MotorType.kBrushless);
  // CANSparkMax intakeR = new CANSparkMax(32, MotorType.kBrushless);

  @Override
  public void teleopPeriodic() 
  {
    // shooter_top.setInverted(true);
    // shooter_bottom.setInverted(true);
    // shooter_top.set(controller.getRawAxis(1));
    // shooter_bottom.set(controller.getRawAxis(3));
    // intakeL.set(controller.getRawAxis(1));
    // intakeR.set(controller.getRawAxis(3));
  }

  @Override
  public void teleopExit() {}

  @Override
  public void testInit() {
    CommandScheduler.getInstance().cancelAll();
  }

  @Override
  public void testPeriodic() {}

  @Override
  public void testExit() {}
}