// 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 edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import swervelib.SwerveDrive;
import swervelib.encoders.CanAndCoderSwerve;
import swervelib.parser.SwerveParser;

public class Drive extends SubsystemBase {
  ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard");
  ShuffleboardLayout avancer = Shuffleboard.getTab("Dashboard")
  .getLayout("Avancer", BuiltInLayouts.kList)
  .withSize(1, 4)
  .withPosition(4, 0);
  ShuffleboardLayout reculer = Shuffleboard.getTab("Dashboard")
  .getLayout("Reculer", BuiltInLayouts.kList)
  .withSize(1, 4)
  .withPosition(3,0);
  SwerveDrive swerveDrive;
  File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
 /*  final CanAndCoderSwerve avantdroitdrive = new CanAndCoderSwerve(Constants.avantdroitDrive); 
  final CanAndCoderSwerve avantgauchedrive = new CanAndCoderSwerve(Constants.avantgaucheDrive); 
  final CanAndCoderSwerve arrieregauchedrive = new CanAndCoderSwerve(Constants.arrieregaucheDrive); 
  final CanAndCoderSwerve arrieredroitdrive = new CanAndCoderSwerve(Constants.arrieredroitDrive); 
  final CanAndCoderSwerve avantdroitangle = new CanAndCoderSwerve(Constants.avantdroitAngle); 
  final CanAndCoderSwerve avantgaucheangle = new CanAndCoderSwerve(Constants.avantgaucheAngle); 
  final CanAndCoderSwerve arrieregaucheangle = new CanAndCoderSwerve(Constants.arrieregaucheAngle); 
  final CanAndCoderSwerve arrieredroitangle = new CanAndCoderSwerve(Constants.arrieredroitAngle); */
  public void drive(double x, double y, double zRotation){
    swerveDrive.drive(new Translation2d(x, y), zRotation, false, false);
  }
  

  /** Creates a new Drive. */
  public Drive() {
    reculer.add("vitesse x", 1);
    reculer.add("vitesse y", 2);
    reculer.add("vitesse z", 3);
    reculer.add("distance", 4);
    avancer.add("vitesse x", 1);
    avancer.add("vitesse y", 2);
    avancer.add("vitesse z", 3);
    avancer.add("distance", 4);
    try {
      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive();
    } 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

  }
}