// 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 com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.DriverStation;
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();
    }
    AutoBuilder.configureHolonomic(swerveDrive::getPose, swerveDrive::resetOdometry, swerveDrive::getRobotVelocity, swerveDrive::setChassisSpeeds, new HolonomicPathFollowerConfig(
      new PIDConstants(5,0,0), new PIDConstants(5,0,0), 4.5,
      0.275,
      new ReplanningConfig()
      ), ()->{
        var alliance = DriverStation.getAlliance();
        if(alliance.isPresent()){
          return alliance.get() == DriverStation.Alliance.Red;
        }
        return false;
         }, this);
  }

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

}

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