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.common.hardware;
019
020import edu.wpi.first.networktables.NetworkTableEvent;
021import java.util.ArrayList;
022import java.util.List;
023import java.util.function.BooleanSupplier;
024import java.util.function.Consumer;
025import org.photonvision.common.hardware.GPIO.CustomGPIO;
026import org.photonvision.common.hardware.GPIO.GPIOBase;
027import org.photonvision.common.hardware.GPIO.pi.PigpioException;
028import org.photonvision.common.hardware.GPIO.pi.PigpioPin;
029import org.photonvision.common.hardware.GPIO.pi.PigpioSocket;
030import org.photonvision.common.logging.LogGroup;
031import org.photonvision.common.logging.Logger;
032import org.photonvision.common.util.TimedTaskManager;
033import org.photonvision.common.util.math.MathUtils;
034
035public class VisionLED {
036    private static final Logger logger = new Logger(VisionLED.class, LogGroup.VisionModule);
037
038    private final int[] ledPins;
039    private final List<GPIOBase> visionLEDs = new ArrayList<>();
040    private final int brightnessMin;
041    private final int brightnessMax;
042    private final PigpioSocket pigpioSocket;
043
044    private VisionLEDMode currentLedMode = VisionLEDMode.kDefault;
045    private BooleanSupplier pipelineModeSupplier;
046
047    private int mappedBrightnessPercentage;
048
049    private final Consumer<Integer> modeConsumer;
050
051    public VisionLED(
052            List<Integer> ledPins,
053            int brightnessMin,
054            int brightnessMax,
055            PigpioSocket pigpioSocket,
056            Consumer<Integer> visionLEDmode) {
057        this.brightnessMin = brightnessMin;
058        this.brightnessMax = brightnessMax;
059        this.pigpioSocket = pigpioSocket;
060        this.modeConsumer = visionLEDmode;
061        this.ledPins = ledPins.stream().mapToInt(i -> i).toArray();
062        ledPins.forEach(
063                pin -> {
064                    if (Platform.isRaspberryPi()) {
065                        visionLEDs.add(new PigpioPin(pin));
066                    } else {
067                        visionLEDs.add(new CustomGPIO(pin));
068                    }
069                });
070        pipelineModeSupplier = () -> false;
071    }
072
073    public void setPipelineModeSupplier(BooleanSupplier pipelineModeSupplier) {
074        this.pipelineModeSupplier = pipelineModeSupplier;
075    }
076
077    public void setBrightness(int percentage) {
078        mappedBrightnessPercentage = MathUtils.map(percentage, 0, 100, brightnessMin, brightnessMax);
079        setInternal(currentLedMode, false);
080    }
081
082    public void blink(int pulseLengthMillis, int blinkCount) {
083        blinkImpl(pulseLengthMillis, blinkCount);
084        int blinkDuration = pulseLengthMillis * blinkCount * 2;
085        TimedTaskManager.getInstance()
086                .addOneShotTask(() -> setInternal(this.currentLedMode, false), blinkDuration + 150);
087    }
088
089    private void blinkImpl(int pulseLengthMillis, int blinkCount) {
090        if (Platform.isRaspberryPi()) {
091            try {
092                setStateImpl(false); // hack to ensure hardware PWM has stopped before trying to blink
093                pigpioSocket.generateAndSendWaveform(pulseLengthMillis, blinkCount, ledPins);
094            } catch (PigpioException e) {
095                logger.error("Failed to blink!", e);
096            } catch (NullPointerException e) {
097                logger.error("Failed to blink, pigpio internal issue!", e);
098            }
099        } else {
100            for (GPIOBase led : visionLEDs) {
101                led.blink(pulseLengthMillis, blinkCount);
102            }
103        }
104    }
105
106    private void setStateImpl(boolean state) {
107        if (Platform.isRaspberryPi()) {
108            try {
109                // stop any active blink
110                pigpioSocket.waveTxStop();
111            } catch (PigpioException e) {
112                logger.error("Failed to stop blink!", e);
113            } catch (NullPointerException e) {
114                logger.error("Failed to blink, pigpio internal issue!", e);
115            }
116        }
117        try {
118            // if the user has set an LED brightness other than 100%, use that instead
119            if (mappedBrightnessPercentage == 100 || !state) {
120                visionLEDs.forEach((led) -> led.setState(state));
121            } else {
122                visionLEDs.forEach((led) -> led.setBrightness(mappedBrightnessPercentage));
123            }
124        } catch (NullPointerException e) {
125            logger.error("Failed to blink, pigpio internal issue!", e);
126        }
127    }
128
129    public void setState(boolean on) {
130        setInternal(on ? VisionLEDMode.kOn : VisionLEDMode.kOff, false);
131    }
132
133    void onLedModeChange(NetworkTableEvent entryNotification) {
134        var newLedModeRaw = (int) entryNotification.valueData.value.getInteger();
135        logger.debug("Got LED mode " + newLedModeRaw);
136        if (newLedModeRaw != currentLedMode.value) {
137            VisionLEDMode newLedMode =
138                    switch (newLedModeRaw) {
139                        case -1 -> newLedMode = VisionLEDMode.kDefault;
140                        case 0 -> newLedMode = VisionLEDMode.kOff;
141                        case 1 -> newLedMode = VisionLEDMode.kOn;
142                        case 2 -> newLedMode = VisionLEDMode.kBlink;
143                        default -> {
144                            logger.warn("User supplied invalid LED mode, falling back to Default");
145                            yield VisionLEDMode.kDefault;
146                        }
147                    };
148            setInternal(newLedMode, true);
149
150            if (modeConsumer != null) modeConsumer.accept(newLedMode.value);
151        }
152    }
153
154    private void setInternal(VisionLEDMode newLedMode, boolean fromNT) {
155        var lastLedMode = currentLedMode;
156
157        if (fromNT) {
158            switch (newLedMode) {
159                case kDefault -> setStateImpl(pipelineModeSupplier.getAsBoolean());
160                case kOff -> setStateImpl(false);
161                case kOn -> setStateImpl(true);
162                case kBlink -> blinkImpl(85, -1);
163            }
164            currentLedMode = newLedMode;
165            logger.info(
166                    "Changing LED mode from \"" + lastLedMode.toString() + "\" to \"" + newLedMode + "\"");
167        } else {
168            if (currentLedMode == VisionLEDMode.kDefault) {
169                switch (newLedMode) {
170                    case kDefault -> setStateImpl(pipelineModeSupplier.getAsBoolean());
171                    case kOff -> setStateImpl(false);
172                    case kOn -> setStateImpl(true);
173                    case kBlink -> blinkImpl(85, -1);
174                }
175            }
176            logger.info("Changing LED internal state to " + newLedMode.toString());
177        }
178    }
179}