// 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.wpilibj2.command.SubsystemBase;
import frc.robot.LimelightHelpers;

public class Limelight3G extends SubsystemBase {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
  NetworkTableEntry pipeline = table.getEntry("pipeline");
  /** Creates a new LimeLight3. */
  public Limelight3G() {
        for(int port = 5800; port <=5807; port++){
      PortForwarder.add(port, "limelight.local", port);
    }
  }
  public double[] getBotPoseBlue(){
    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
    NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
    double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
    return BotPose;
    }
    public double[] getBotPoseRed(){
      NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
    NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
    double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
    return BotPose;
    }
    public double getTx(){
      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
      NetworkTableEntry tx = table.getEntry("tx");
      return tx.getDouble(0.0);
    }
    public double getTId(){
      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
      NetworkTableEntry tid = table.getEntry("tid");
      return tid.getDouble(0.0);
    }
    public double getTA(){
      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
      NetworkTableEntry ta = table.getEntry("ta");
      return ta.getDouble(0.0);
    }
    public boolean getV(){
      return LimelightHelpers.getTV("limelight-tag");
    }
    public double Calcule(double x1, double x2, double y1, double y2, double angle)
    {
      // if(x1 > x2){
      //   if(y1 > y2){
      //     return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
      //   }
      //   else{
      //     return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle;
      //   }
      // }
      // else{
      //   if(y1 > y2){
      //     return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
      //   }
      //   else{
      //     return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
      //   }
      // }
      if(y1 > y2){
          return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
        }
        else{
          return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle;
        }
    }
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}