package frc.robot.subsystems;

import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import java.util.function.Consumer;

import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;

import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.CompetitionConstants;
import frc.robot.constants.PhotonConstants;
import frc.robot.utilities.PhotonVisionConfig;
import frc.robot.utilities.VisualPose;

/**
 * This "Subsystem" is not actually a Subsystem. The intent is for this to be treated as
 * a "resource", that is, something that is not inherently a physical mechanism to be controlled.
 * 
 * A "resource" in this instance should be thought of as something that can be safely shared
 * by other Subsystems generally without collision if more that one Subsystem requires the 
 * "resource" at any given time.
 * 
 * Resources should <i>NOT</i> produce Commands, they should not have a default Command.
 * Resources do not have behaviors, and because Commands are in of themselves behaviors,
 * this class should not have Commands. 
 * 
 * Part of the thinking behind creating the PhotonVision components this way is to rely
 * on the CommandScheduler to call periodic. If this weren't the case, some other subsystem
 * would have to manage calling for periodic updates, while still sharing the resource with 
 * other subsystems <i>somehow</i>. 
 * 
 * This class is dynamic, by adding or removing PhotonVisionConfig objects to the "configs"
 * List in the PhotonConstants file, you change what is set up internally in this class. 
 * 1 config means 1 camera, 1 estimator, 1 stored pipeline result, 2 configs means 2 cameras,
 * 2 estimators, etc. etc. 
 */
public class PhotonVision extends SubsystemBase {
    private PhotonCamera[] cameras;
    private PhotonPoseEstimator[] estimators;
    private List<PhotonPipelineResult> latestResults;

    private ArrayList<Consumer<VisualPose>> poseEstimateConsumers;

    public PhotonVision() {
        cameras = new PhotonCamera[PhotonConstants.configs.size()];
        estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()];
        latestResults = new ArrayList<PhotonPipelineResult>();

        for(int i = 0; i < PhotonConstants.configs.size(); i++) {
            cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName());
            estimators[i] = new PhotonPoseEstimator(
                CompetitionConstants.kTagLayout, 
                PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
                PhotonConstants.configs.get(i).robotToCamera()
            );
            latestResults.add(null);
        }

        poseEstimateConsumers = new ArrayList<Consumer<VisualPose>>();
    }

    @Override
    public void periodic() {
        for(int i = 0; i < cameras.length; i++) {
            List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();

            if(!results.isEmpty()) {
                latestResults.set(i, results.get(results.size() - 1));
            }

            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));

            if(!pose.isEmpty()) {
                VisualPose visualPose = new VisualPose(
                    cameras[i].getName(),
                    pose.get().estimatedPose.toPose2d(), 
                    pose.get().timestampSeconds
                );

                for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
                    consumer.accept(visualPose);
                }
            }
        }
    }

    /**
     * Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot.
     * 
     * All cameras fields of view are observed, if no camera can see the given tag ID, this
     * method will return Optional.empty().
     * 
     * Note that this method has no minimum confidence threshold for a tag. This means that 
     * if one camera thinks it sees the tag, even with very low confidence, it'll still return
     * some sort of pose. 
     * 
     * @param tagID The ID of the tag to look for in the latest results from all cameras
     * @return An Optional object containing a Pose3d object, or Optional.empty() if
     *   the tag is not present anywhere in the robots field of view. 
     */
    public Optional<Pose3d> getBestPoseForTag(int tagID) {
        PhotonVisionConfig config = null;
        Transform3d bestCameraToTarget = null;
        float bestConfidence = -1;

        for(int cameraIndex = 0; cameraIndex < latestResults.size(); cameraIndex++) {
            if(latestResults.get(cameraIndex) == null) {
                continue;
            }

            for(PhotonTrackedTarget target: latestResults.get(cameraIndex).getTargets()) {
                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectConfidence()) {
                    continue;
                }

                config = PhotonConstants.configs.get(cameraIndex);
                bestCameraToTarget = target.bestCameraToTarget;
                bestConfidence = target.getDetectedObjectConfidence();
            }
        }

        if(bestCameraToTarget == null) {
            return Optional.empty();
        }

        // This is based on what PhotonVision does for multitag Pose estimation
        // See PhotonPoseEstimator.multiTagOnCoprocStrategy
        // TODO This doesn't currently account for the offset of the tag relative to say the hub
        return Optional.of(Pose3d.kZero
            .plus(bestCameraToTarget.inverse())
            .relativeTo(CompetitionConstants.kTagLayout.getOrigin())
            .plus(config.robotToCamera().inverse())
            .plus(CompetitionConstants.kRobotToShooter));
    }

    /**
     * Add a Consumer of VisualPose records to the PhotonVision resource. 
     * 
     * Each consumer will receive a VisualPose object when any camera produces a new
     * VisualPose. 
     * 
     * The number of Poses produced in a given 20ms cycle is the same number as how many
     * cameras there are on the robot, as currently all cameras configuration will generate
     * a Pose2d
     * 
     * @param consumer The lambda or functional reference that will consume Poses produced
     *   by the PhotonVision resource.
     */
    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
        poseEstimateConsumers.add(consumer);
    }

    /*public Trigger tagPrescenseTrigger(int targetTag) {
        return new Trigger(() -> {
            return List.of(latestResults).stream()
                .filter((p) -> p != null)
                .anyMatch((p) -> {
                    return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> {
                        return i == targetTag;
                    });
                });
            });
    }*/
/* 
    @Override
    public OptionalDouble getTagDistanceFromCameraByID(int id) {
        if (latestResult == null) {
            return OptionalDouble.empty();
        }

        if (!latestResult.hasTargets()) {
            return OptionalDouble.empty();
        }

        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
        
        if (desiredTarget.isEmpty()) {
            return OptionalDouble.empty();
        }

        return OptionalDouble.of(
            PhotonUtils.calculateDistanceToTargetMeters(
                cameraHeightMeters, 
                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
                cameraPitchRadians, 
                Units.degreesToRadians(desiredTarget.get().getPitch()))
        );
    }

    @Override
    public OptionalDouble getTagPitchByID(int id) {
        if(latestResult == null) {
            OptionalDouble.empty();
        }
        
        if (!latestResult.hasTargets()) {
            return OptionalDouble.empty();
        }

        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);

        if (desiredTarget.isEmpty()) {
            return OptionalDouble.empty();
        }

        return OptionalDouble.of(
            desiredTarget.get().getPitch()
        );
    }

    @Override
    public OptionalDouble getTagYawByID(int id) {
        if(latestResult == null) {
            OptionalDouble.empty();
        }
        
        if (!latestResult.hasTargets()) {
            return OptionalDouble.empty();
        }

        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);

        if (desiredTarget.isEmpty()) {
            return OptionalDouble.empty();
        }

        return OptionalDouble.of(
            desiredTarget.get().getYaw()
        );
    }

    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
        for (PhotonTrackedTarget target : targets) {
            if (target.getFiducialId() == id) {
                return Optional.of(target);
            }
        }

        return Optional.empty();
    }

    @Override
    public int[] getVisibleTagIDs() {
        if(latestResult == null) {
            return new int[] {};
        }

        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
    }
    */
}