001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.vision.processes;
019
020import edu.wpi.first.cscore.VideoMode;
021import java.util.HashMap;
022import org.photonvision.common.configuration.CameraConfiguration;
023import org.photonvision.common.logging.LogGroup;
024import org.photonvision.common.logging.Logger;
025import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
026import org.photonvision.vision.frame.FrameStaticProperties;
027
028public abstract class VisionSourceSettables {
029    protected Logger logger;
030
031    private final CameraConfiguration configuration;
032
033    protected VisionSourceSettables(CameraConfiguration configuration) {
034        this.configuration = configuration;
035        this.logger =
036                new Logger(VisionSourceSettables.class, configuration.nickname, LogGroup.VisionModule);
037    }
038
039    protected FrameStaticProperties frameStaticProperties = null;
040    protected HashMap<Integer, VideoMode> videoModes = new HashMap<>();
041
042    public CameraConfiguration getConfiguration() {
043        return configuration;
044    }
045
046    // If the device has been connected at least once, and we cached properties
047    protected boolean cameraPropertiesCached = false;
048
049    /**
050     * Runs exactly once the first time that the underlying device goes from disconnected to connected
051     */
052    public void onCameraConnected() {
053        cameraPropertiesCached = true;
054    }
055
056    public abstract void setExposureRaw(double exposureRaw);
057
058    public abstract double getMinExposureRaw();
059
060    public abstract double getMaxExposureRaw();
061
062    public abstract void setAutoExposure(boolean cameraAutoExposure);
063
064    public abstract void setWhiteBalanceTemp(double temp);
065
066    public abstract void setAutoWhiteBalance(boolean autowb);
067
068    public abstract void setBrightness(int brightness);
069
070    public abstract void setGain(int gain);
071
072    // Pretty uncommon so instead of abstract this is just a no-op by default
073    // Overridden by cameras with AWB gain support
074    public void setRedGain(int red) {}
075
076    public void setBlueGain(int blue) {}
077
078    public abstract VideoMode getCurrentVideoMode();
079
080    public void setVideoModeInternal(int index) {
081        if (!getAllVideoModes().isEmpty()) setVideoMode(getAllVideoModes().get(index));
082    }
083
084    public void setVideoMode(VideoMode mode) {
085        logger.info(
086                "Setting video mode to "
087                        + "FPS: "
088                        + mode.fps
089                        + " Width: "
090                        + mode.width
091                        + " Height: "
092                        + mode.height
093                        + " Pixel Format: "
094                        + mode.pixelFormat);
095        setVideoModeInternal(mode);
096        calculateFrameStaticProps();
097    }
098
099    protected abstract void setVideoModeInternal(VideoMode videoMode);
100
101    @SuppressWarnings("unused")
102    public void setVideoModeIndex(int index) {
103        setVideoMode(videoModes.get(index));
104    }
105
106    public abstract HashMap<Integer, VideoMode> getAllVideoModes();
107
108    public double getFOV() {
109        return configuration.FOV;
110    }
111
112    public void setFOV(double fov) {
113        logger.info("Setting FOV to " + fov);
114        configuration.FOV = fov;
115        calculateFrameStaticProps();
116    }
117
118    public void addCalibration(CameraCalibrationCoefficients calibrationCoefficients) {
119        configuration.addCalibration(calibrationCoefficients);
120        calculateFrameStaticProps();
121    }
122
123    protected void calculateFrameStaticProps() {
124        var videoMode = getCurrentVideoMode();
125        this.frameStaticProperties =
126                new FrameStaticProperties(
127                        videoMode,
128                        getFOV(),
129                        configuration.calibrations.stream()
130                                .filter(
131                                        it ->
132                                                it.unrotatedImageSize.width == videoMode.width
133                                                        && it.unrotatedImageSize.height == videoMode.height)
134                                .findFirst()
135                                .orElse(null));
136    }
137
138    public FrameStaticProperties getFrameStaticProperties() {
139        return frameStaticProperties;
140    }
141
142    public abstract double getMinWhiteBalanceTemp();
143
144    public abstract double getMaxWhiteBalanceTemp();
145}