PhotonVision/Main/src/main/java/com/chameleonvision/vision/VisionManager.java

package com.chameleonvision.vision;

import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.util.Platform;
import com.chameleonvision.vision.camera.CameraProcess;
import com.chameleonvision.vision.camera.USBCameraProcess;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.UsbCameraInfo;
import org.opencv.videoio.VideoCapture;

import java.io.IOException;
import java.util.*;
import java.util.stream.Collectors;

public class VisionManager {
    private VisionManager() {
    }

    private static final LinkedHashMap<String, UsbCameraInfo> usbCameraInfosByCameraName = new LinkedHashMap<>();
    private static final LinkedList<CameraConfig> loadedCameraConfigs = new LinkedList<>();
    private static final LinkedList<VisionProcessManageable> visionProcesses = new LinkedList<>();

    private static class VisionProcessManageable {
        public final int index;
        public final String name;
        public final VisionProcess visionProcess;

        public VisionProcessManageable(int index, String name, VisionProcess visionProcess) {
            this.index = index;
            this.name = name;
            this.visionProcess = visionProcess;
        }
    }

    private static VisionProcess currentUIVisionProcess;

    public static boolean initializeSources() {
        int suffix = 0;
        for (UsbCameraInfo info : UsbCamera.enumerateUsbCameras()) {
            VideoCapture cap = new VideoCapture(info.dev);
            if (cap.isOpened()) {
                cap.release();
                String name = info.name;
                while (usbCameraInfosByCameraName.containsKey(name)) {
                    suffix++;
                    name = String.format("%s (%d)", name, suffix);
                }
                usbCameraInfosByCameraName.put(name, info);
            }
        }

        if (usbCameraInfosByCameraName.isEmpty()) {
            return false;
        }

        // load the config
        List<CameraConfig> preliminaryConfigs = new ArrayList<>();

        usbCameraInfosByCameraName.values().forEach((cameraInfo) -> {
            String truePath;

            if (Platform.CurrentPlatform.isWindows()) {
                truePath = cameraInfo.path;
            } else {
                truePath = Arrays.stream(cameraInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst().orElse(cameraInfo.path);
            }

            preliminaryConfigs.add(new CameraConfig(truePath, cameraInfo.name));
        });

        loadedCameraConfigs.addAll(ConfigManager.initializeCameraConfig(preliminaryConfigs));

        // TODO: (HIGH) Load pipelines from json
//        UsbCameraInfosByCameraName.forEach((cameraName, cameraInfo) -> {
//            Path cameraConfigFolder = Paths.get(CamConfigPath.toString(), String.format("%s\\", cameraName));
//            Path cameraConfigPath = Paths.get(cameraConfigFolder.toString(), String.format("%s.json", cameraName));
//            Path cameraPipelinesPath = Paths.get(cameraConfigFolder.toString(), "pipelines.json");
//            Path cameraDrivermodePath = Paths.get(cameraConfigFolder.toString(), "drivermode.json");

        return true;
    }

    public static boolean initializeProcesses() {
        for (int i = 0; i < loadedCameraConfigs.size(); i++) {
            CameraConfig config = loadedCameraConfigs.get(i);
            CameraProcess camera = new USBCameraProcess(config);
            VisionProcess process = new VisionProcess(camera, config.name);
            visionProcesses.add(new VisionProcessManageable(i, config.name, process));
        }
        currentUIVisionProcess = getVisionProcessByIndex(0);
        return true;
    }

    public static void startProcesses() {
        visionProcesses.forEach((vpm) -> {
            vpm.visionProcess.start();
        });
    }

    public static VisionProcess getCurrentUIVisionProcess() {
        return currentUIVisionProcess;
    }

    public static void setCurrentProcessByIndex(int processIndex) {
        if (processIndex > visionProcesses.size() - 1) {
            return;
        }

        currentUIVisionProcess = getVisionProcessByIndex(0);
    }

    public static VisionProcess getVisionProcessByIndex(int processIndex) {
        if (processIndex > visionProcesses.size() - 1) {
            return null;
        }

        VisionProcessManageable vpm =  visionProcesses.stream().filter(manageable -> manageable.index == processIndex).findFirst().orElse(null);
        return vpm != null ? vpm.visionProcess : null;
    }

    public static List<String> getAllCameraNicknames() {
        return visionProcesses.stream().map(vpm -> vpm.visionProcess.getCamera()
                .getProperties().getNickname()).collect(Collectors.toList());
    }

    public static List<String> getCurrentCameraPipelineNicknames() {
        return currentUIVisionProcess.getPipelines().stream().map(cvPipeline -> cvPipeline.settings.nickname).collect(Collectors.toList());
    }

    public static void saveCameras() {
        visionProcesses.forEach((vpm) -> {
            VisionProcess process = vpm.visionProcess;
            String cameraName = process.getCamera().getProperties().name;
            List<CVPipelineSettings> pipelines = process.getPipelines().stream().map(cvPipeline -> cvPipeline.settings).collect(Collectors.toList());
            CVPipelineSettings driverMode = process.getDriverModeSettings();
            CameraConfig config = CameraConfig.fromUSBCameraProcess((USBCameraProcess) process.getCamera());
            try {
                ConfigManager.saveCameraPipelines(cameraName, pipelines);
                ConfigManager.saveCameraDriverMode(cameraName, driverMode);
                ConfigManager.saveCameraConfig(cameraName, config);
            } catch (IOException e) {
                e.printStackTrace();
            }
        });
    }

    public static List<String> getCurrentCameraResolutionList() {
        return currentUIVisionProcess.getCamera().getProperties().getVideoModes().stream().map(Helpers::VideoModeToString).collect(Collectors.toList());
    }

    public static int getCurrentUIVisionProcessIndex() {
        VisionProcessManageable vpm = visionProcesses.stream().filter(v -> v.visionProcess == currentUIVisionProcess).findFirst().orElse(null);
        return vpm != null ? vpm.index : -1;
    }
}