// 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 edu.wpi.first.cameraserver.CameraServer;


import org.photonvision.PhotonCamera;
import org.photonvision.common.hardware.VisionLEDMode;


import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;


public class Limelight extends SubsystemBase {
 
  PhotonCamera limelight = new PhotonCamera("limelight");
  /** Creates a new Limelight. */
  public Limelight() {
    CameraServer.startAutomaticCapture();
    PortForwarder.add(5800, "photonvision.local", 5800);
  }

  public void cube()  {
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setPipelineIndex(3);
  }

  public void cone() {
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setPipelineIndex(2);
  }

  public void apriltag() {
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setPipelineIndex(0);
  }

  public void tape() {
    limelight.setLED(VisionLEDMode.kOn);
    limelight.setPipelineIndex(1);
  }

  public double getYaw() {
    var result = limelight.getLatestResult();
    if(result.hasTargets()){
    return   -result.getBestTarget().getYaw()/30;
    }
    return 0;
  
  }
  public void pilote(){
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setDriverMode(true);
  }
  public void joueurhumain(){
    limelight.setLED(VisionLEDMode.kOn);
  }
  public void joueurhumain1(){
    limelight.setLED(VisionLEDMode.kOff);
  }
  @Override
  public void periodic() {
  }
}