// 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.net.PortForwarder;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.LimelightHelpers;

public class Limelight3 extends SubsystemBase {
  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
  NetworkTableEntry pipeline = table.getEntry("pipeline");
  /** Creates a new Limelight3. */
  public Limelight3() {
    
    for(int port = 5800; port <=5807; port++){
      PortForwarder.add(port, "limelight.local", port);
    }
  }
  public double[] getTargetPose(){
    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
  NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
  double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
  return targetPose;
  }
  public double[] getBotPoseBlue(){
  NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
  double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
  return BotPose;
  }
  public double[] getBotPoseRed(){
    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
  double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
  return BotPose;
  }
  public double getTx(){
    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
    NetworkTableEntry tx = table.getEntry("tx");
    return tx.getDouble(0.0);
  }
  public double getTA(){
    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
    NetworkTableEntry ta = table.getEntry("ta");
    return ta.getDouble(0.0);
  }
  public double getTId(){
    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
    NetworkTableEntry tid = table.getEntry("tid");
    return tid.getDouble(0.0);
  }
  public double getRx(){
    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
    NetworkTableEntry rx = table.getEntry("rx");
    return rx.getDouble(0.0);
  }
  public boolean getV(){
    return LimelightHelpers.getTV("limelight-balon");
  }
  public void Apriltag(){
    pipeline.setNumber(1);
  }
  public void Forme(){
    pipeline.setNumber(0);
  }
  public double Calcule(double x1, double x2, double y1, double y2, double angle)
  {
    if (x1 > 4)
    {
      return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90;
    }
    else
    {
        return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90;
    }
  }
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}