/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

// import necessary packages/modules
package frc.robot.subsystems;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;

import com.kauailabs.navx.frc.AHRS;
import frc.robot.Robot;
import frc.robot.RobotMap;
import frc.robot.commands.Drive;

public class DriveSubsystem extends Subsystem {

  // instantiate navX
  public static AHRS navX;

  // instantiate variables
  public static WPI_TalonSRX talon_fl;
  public static WPI_TalonSRX talon_fr;
  public static WPI_VictorSPX victor_bl;
  public static WPI_VictorSPX victor_br;

  public static DifferentialDrive robotDrive;
  public boolean driveInverted;
  private double rumblePower;

  // instantiate kP, kF, kD, and kI constants
  double kP;
  double kF;
  double kD;
  double kI;

  // class constructor
  public DriveSubsystem() {
    super("Drive Subsystem");

    // yeet yeet navX
    navX = new AHRS();
    // instantiate motor controllers
    talon_fl = new WPI_TalonSRX(RobotMap.talon_fl);
    talon_fr = new WPI_TalonSRX(RobotMap.talon_fr);
    victor_bl = new WPI_VictorSPX(RobotMap.victor_bl);
    victor_br = new WPI_VictorSPX(RobotMap.victor_br);
    robotDrive = new DifferentialDrive(talon_fl, talon_fr);

    // default drivetrain inversion
    driveInverted = true;

    // configure motor controllers
    victor_bl.follow(talon_fl);
    victor_br.follow(talon_fr);
    talon_fl.setInverted(driveInverted);
    talon_fr.setInverted(driveInverted);
    victor_bl.setInverted(driveInverted);
    victor_br.setInverted(driveInverted);

    talon_fl.configPeakCurrentLimit(80);
    talon_fr.configPeakCurrentLimit(80);
    talon_fl.configPeakCurrentDuration(50);
    talon_fr.configPeakCurrentDuration(50);
    talon_fl.enableCurrentLimit(true);
    talon_fr.enableCurrentLimit(true);

    talon_fl.setSafetyEnabled(false);
    talon_fr.setSafetyEnabled(false);
    victor_bl.setSafetyEnabled(false);
    victor_br.setSafetyEnabled(false);

    // set values of PID constants
    kP = 0.000001;
    kF = 0.0;
    kD = 0.0;
    kI = 0.0;
  }

  // curve drive method
  public void curveDrive(double speed, double rotation, boolean isQuickTurn) {
    robotDrive.curvatureDrive(speed, rotation, isQuickTurn);
  }

  // turn
  public void turnPID(double desiredAngle) {
      double errorAngle = desiredAngle - navX.getAngle();
      double PValue = 0;
      double DValue = 0;
      double IValue = 0;

      while(errorAngle != 0) {
          PValue = errorAngle * kP;
          DValue = (errorAngle - navX.getAngle()) * kD;
          IValue += errorAngle;
          double rotationValue = (PValue + kF + DValue + IValue);
          robotDrive.curvatureDrive(0, rotationValue, true);
          errorAngle = desiredAngle - navX.getAngle();
      }
  }

  // invert drive direction
  public void invertDirection() {
    driveInverted = !driveInverted;

    talon_fl.setInverted(driveInverted);
    talon_fr.setInverted(driveInverted);
    victor_bl.setInverted(driveInverted);
    victor_br.setInverted(driveInverted);
  }

  @Override
  public void initDefaultCommand() {
    //setDefaultCommand(new Drive());
  }

}

















/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.commands;

import edu.wpi.first.wpilibj.command.Command;

import frc.robot.Robot;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.OI;

public class Drive extends Command {

  public double rumblePower;

  public Drive() {
    requires(Robot.s_drive);

    rumblePower = 0;
  }

  @Override
  protected void initialize() {
  }

  @Override
  protected void execute() {
    // invert drive direction
    if (Robot.oi.driveController.getYButtonPressed()) {
      Robot.s_drive.invertDirection();
    }

    if (Robot.s_drive.driveInverted) {
      if (!Robot.oi.driveController.getXButton()) { // when not quick turning
        Robot.s_drive.curveDrive(Robot.oi.getTriggerMagnitude(Robot.oi.driveController),
        Robot.oi.getXMagnitude(Robot.oi.driveController) * 50,
        false);
      } else { // when quick turning
        Robot.s_drive.curveDrive(Robot.oi.getTriggerMagnitude(Robot.oi.driveController),
        Robot.oi.getXMagnitude(Robot.oi.driveController) * 0.8,
        true);
      }
    } else {
      if (!Robot.oi.driveController.getXButton()) { // when not quick turning
        Robot.s_drive.curveDrive(Robot.oi.getTriggerMagnitude(Robot.oi.driveController),
        Robot.oi.getXMagnitude(Robot.oi.driveController) * -50,
        false);
      } else { // when quick turning
        Robot.s_drive.curveDrive(Robot.oi.getTriggerMagnitude(Robot.oi.driveController),
        Robot.oi.getXMagnitude(Robot.oi.driveController) * -0.8,
        true);
      }
    }

    /*
    if(Robot.oi.driveController.getPOV() == 0) {
      Robot.s_drive.turnPID(0);
    } else if(Robot.oi.driveController.getPOV() == 90) {
      Robot.s_drive.turnPID(90);
    } else if(Robot.oi.driveController.getPOV() == 180) {
      Robot.s_drive.turnPID(180);
    } else if(Robot.oi.driveController.getPOV() ==  270) {
      Robot.s_drive.turnPID(270);
    } */
  }

  @Override
  protected boolean isFinished() {
    return false;
  }

  @Override
  protected void end() {
  }

  @Override
  protected void interrupted() {
  }
}