PhotonVision/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java

package com.chameleonvision.vision.camera;

import com.chameleonvision.Main;
import com.chameleonvision.settings.Platform;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.web.Server;
import com.chameleonvision.web.ServerHandler;
import edu.wpi.cscore.*;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.opencv.core.Mat;

import java.nio.channels.Pipe;
import java.util.*;
import java.util.stream.Collectors;
import java.util.stream.IntStream;

public class Camera {

    private static final double DEFAULT_FOV = 60.8;
    private static final StreamDivisor DEFAULT_STREAMDIVISOR = StreamDivisor.none;
    private static final int DEFAULT_EXPOSURE = 50;
    private static final int DEFAULT_BRIGHTNESS = 50;
    private static final int MINIMUM_FPS = 30;
    private static final int MINIMUM_WIDTH = 320;
    private static final int MINIMUM_HEIGHT = 200;
    private static final int MAX_INIT_MS = 1500;
    private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG);


    public final String name;
    public final String path;

    private String nickname;

    private final UsbCamera UsbCam;
    private final VideoMode[] availableVideoModes;

    private final CameraServer cs = CameraServer.getInstance();
    private final CvSink cvSink;
    private final Object cvSourceLock = new Object();
    private CvSource cvSource;
    private Double FOV;
    private StreamDivisor streamDivisor;
    private CameraValues camVals;
    private CamVideoMode camVideoMode;
    private int currentPipelineIndex;
    private List<Pipeline> pipelines;

    //Driver mode camera settings
    public int driverExposure;
    public int driverBrightness;
    public boolean isDriver;

    public Camera(String cameraName) {
        this(cameraName, DEFAULT_FOV);
    }

    public Camera(String cameraName, double fov) {
        this(cameraName, CameraManager.AllUsbCameraInfosByName.get(cameraName), fov);
    }

    public Camera(String cameraName, UsbCameraInfo usbCameraInfo, double fov) {
        this(cameraName, usbCameraInfo, fov, DEFAULT_STREAMDIVISOR);
    }

    public Camera(String cameraName, UsbCameraInfo usbCamInfo, double fov, StreamDivisor divisor) {
        this(cameraName, usbCamInfo, fov, new ArrayList<>(), 0, divisor, DEFAULT_EXPOSURE, DEFAULT_BRIGHTNESS,false);
    }

    public Camera(String cameraName, double fov, List<Pipeline> pipelines, int videoModeIndex, StreamDivisor divisor, int driverExposure, int driverBrightness,boolean isDriver) {
        this(cameraName, CameraManager.AllUsbCameraInfosByName.get(cameraName), fov, pipelines, videoModeIndex, divisor, driverExposure, driverBrightness, isDriver);
    }

    public Camera(String cameraName, double fov, int videoModeIndex, StreamDivisor divisor, int driverExposure, int driverBrightness,boolean isDriver) {
        this(cameraName, fov, new ArrayList<>(), videoModeIndex, divisor, driverExposure, driverBrightness,isDriver);
    }

    public Camera(String cameraName, UsbCameraInfo usbCamInfo, double fov, List<Pipeline> pipelines, int videoModeIndex, StreamDivisor divisor, int driverExposure, int driverBrightness,boolean isDriver) {
        FOV = fov;
        name = cameraName;

        if (Platform.getCurrentPlatform().isWindows()) {
            path = usbCamInfo.path;
        } else {
            var truePath = Arrays.stream(usbCamInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst();
            path = truePath.isPresent() ? truePath.toString() : null;
        }

        streamDivisor = divisor;
        UsbCam = new UsbCamera(name, path);

        this.pipelines = pipelines;

        // set up video modes according to minimums
        if (Platform.getCurrentPlatform() == Platform.WINDOWS_64 && !UsbCam.isConnected()) {
            System.out.print("Waiting on camera... ");
            long initTimeout = System.nanoTime();
            while (!UsbCam.isConnected()) {
                if (((System.nanoTime() - initTimeout) / 1e6) >= MAX_INIT_MS) {
                    break;
                }
            }
            var initTimeMs = (System.nanoTime() - initTimeout) / 1e6;
            System.out.printf("Camera initialized in %.2fms\n", initTimeMs);
        }
        var trueVideoModes = UsbCam.enumerateVideoModes();
        availableVideoModes = Arrays.stream(trueVideoModes).filter(v ->
                v.fps >= MINIMUM_FPS && v.width >= MINIMUM_WIDTH && v.height >= MINIMUM_HEIGHT && ALLOWED_PIXEL_FORMATS.contains(v.pixelFormat)).toArray(VideoMode[]::new);
        if (availableVideoModes.length == 0) {
            System.err.println("Camera not supported!");
            throw new RuntimeException(new CameraException(CameraException.CameraExceptionType.BAD_CAMERA));
        }
        if (videoModeIndex <= availableVideoModes.length - 1) {
            setCamVideoMode(videoModeIndex, false);
        } else {
            setCamVideoMode(0, false);
        }

        cvSink = cs.getVideo(UsbCam);
        cvSource = cs.putVideo(name, camVals.ImageWidth, camVals.ImageHeight);

        //Driver mode settings
        this.isDriver = isDriver;
        this.driverBrightness=driverBrightness;
        this.driverExposure=driverExposure;
    }

    VideoMode[] getAvailableVideoModes() {
        return availableVideoModes;
    }

    public int getStreamPort() {
        var s = (MjpegServer) cs.getServer("serve_" + name);
        return s.getPort();
    }

    public void setCamVideoMode(int videoMode, boolean updateCvSource) {
        setCamVideoMode(new CamVideoMode(availableVideoModes[videoMode]), updateCvSource);
    }

    private void setCamVideoMode(CamVideoMode newVideoMode, boolean updateCvSource) {
        var prevVideoMode = this.camVideoMode;
        this.camVideoMode = newVideoMode;
        UsbCam.setVideoMode(newVideoMode.getActualPixelFormat(), newVideoMode.width, newVideoMode.height, newVideoMode.fps);

        // update camera values
        camVals = new CameraValues(this);
        if (prevVideoMode != null && !prevVideoMode.equals(newVideoMode) && updateCvSource) { //  if resolution changed
            synchronized (cvSourceLock) {
                cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height);
            }
            ServerHandler.sendFullSettings();
        }
    }

    public void addPipeline() {
        Pipeline p = new Pipeline();
        p.nickname = "New pipeline " + pipelines.size();
        addPipeline(p);
    }

    public void addPipeline(Pipeline p) {
        this.pipelines.add(p);
    }

    public void deletePipeline(int index) {
        pipelines.remove(index);
    }

    public void deletePipeline() {
        deletePipeline(getCurrentPipelineIndex());
    }

    public Pipeline getCurrentPipeline() {
        return getPipelineByIndex(currentPipelineIndex);
    }

    public Pipeline getPipelineByIndex(int pipelineIndex) {
        return pipelines.get(pipelineIndex);
    }

    public int getCurrentPipelineIndex() {
        return currentPipelineIndex;
    }

    public void setCurrentPipelineIndex(int pipelineNumber) {
        if (pipelineNumber - 1 > pipelines.size()) return;
        currentPipelineIndex = pipelineNumber;
    }

    public StreamDivisor getStreamDivisor() {
        return streamDivisor;
    }

    public void setStreamDivisor(int divisor) {
        streamDivisor = StreamDivisor.values()[divisor];
    }

    public List<Pipeline> getPipelines() {
        return pipelines;
    }

    public List<String> getPipelinesNickname() {
        var pipelines = getPipelines();
        return pipelines.stream().map(pipeline -> pipeline.nickname).collect(Collectors.toList());
    }

    public CamVideoMode getVideoMode() {
        return camVideoMode;
    }

    public int getVideoModeIndex() {
        return IntStream.range(0, availableVideoModes.length)
                .filter(i -> camVideoMode.equals(availableVideoModes[i]))
                .findFirst()
                .orElse(-1);
    }

    public double getFOV() {
        return FOV;
    }

    public void setFOV(Number fov) {
        FOV = fov.doubleValue();
        camVals = new CameraValues(this);
    }

    public void setDriverMode(boolean state)
    {
        isDriver=state;
        if(isDriver){
            UsbCam.setBrightness(driverBrightness);//We call setBrightness because it updates after 2 calls
            UsbCam.setBrightness(driverBrightness);//Check it after we update to 2020 libraries
            try{UsbCam.setExposureManual(driverExposure);}
            catch (VideoException e)
            {
                System.out.println("Exposure change isn't supported");
            }
        }
        else{
            UsbCam.setBrightness(getCurrentPipeline().brightness);
            UsbCam.setBrightness(getCurrentPipeline().brightness);
            try{UsbCam.setExposureManual(getCurrentPipeline().exposure);}
            catch (VideoException e)
            {
                System.out.println("Exposure change isn't supported");
            }
        }
    }

    public boolean getDriverMode()
    {
        return isDriver;
    }

    public int getBrightness() {
        return UsbCam.getBrightness();
    }



    public void setBrightness(int brightness) {
        if (isDriver)
            driverBrightness=brightness;
        else
            getCurrentPipeline().brightness = brightness;
        UsbCam.setBrightness(brightness);
    }

    public void setExposure(int exposure) {
        if (isDriver)
            driverExposure=exposure;
        else
            getCurrentPipeline().exposure = exposure;
        try {
            UsbCam.setExposureManual(exposure);
        } catch (VideoException e) {
            System.err.println("Camera Does not support exposure change");
        }
    }

    public long grabFrame(Mat image) {
        return cvSink.grabFrame(image);
    }

    public CameraValues getCamVals() {
        return camVals;
    }

    public void putFrame(Mat image) {
        synchronized (cvSourceLock) {
            cvSource.putFrame(image);
        }
    }

    public List<String> getResolutionList() {
        return Arrays.stream(availableVideoModes)
                .map(res -> String.format("%sx%s@%sFPS, %s", res.width, res.height, res.fps, res.pixelFormat.toString()))
                .collect(Collectors.toList());
    }

    public void setNickname(String newNickname) {
        //Deletes old camera nt table
        NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + this.nickname).getInstance().deleteAllEntries();
        nickname = newNickname;
        if (CameraManager.AllVisionProcessesByName.containsKey(this.name)) {
            NetworkTable newNT = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + this.nickname);
            CameraManager.AllVisionProcessesByName.get(this.name).resetNT(newNT);
        }
    }

    public String getNickname() {
        return nickname == null ? name : nickname;
    }

}