// 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.subsystem;

import java.io.File;
import java.io.IOException;

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 {
  /** Creates a new Drive. */
  
   SwerveDrive swerveDrive;
  File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
  public void drive(double x, double y, double zRotation){
    swerveDrive.drive(new Translation2d(x*2, y*2), zRotation, true, false);
  }
  
  public Drive() {

     try {
      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
    } catch (IOException e) {
      e.printStackTrace();
    }
  }

  public SwerveModulePosition[] distance(){
    return swerveDrive.getModulePositions();
  }
public void reset(){

}

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