// 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.Subsystems;
import java.io.File;
import java.io.IOException;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import swervelib.SwerveDrive;
import swervelib.parser.SwerveParser;

public class Drive extends SubsystemBase {

    SwerveDrive swerveDrive;

  File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
  
  Pigeon2 Gyro = new Pigeon2(0);
   
  public void drive(double x, double y, double zRotation){
    swerveDrive.drive(new Translation2d(x*5, y*5), zRotation*4, true, false);
  }
  
  public Drive() {
     try {
      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(5);
      swerveDrive.setHeadingCorrection(true);
    } catch (IOException e) {
      e.printStackTrace();
    }
  }
  public void restgyroscope(){
    Gyro.reset();
  }
  public SwerveModulePosition[] distance(){
    return swerveDrive.getModulePositions();
  }

  
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}