001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.signals;
008
009import static edu.wpi.first.units.Units.Degrees;
010
011import java.util.Objects;
012import java.util.Optional;
013
014import edu.wpi.first.units.measure.Angle;
015import edu.wpi.first.wpilibj.util.Color;
016import edu.wpi.first.wpilibj.util.Color8Bit;
017
018/**
019 * Represents an RGBW color that can be applied to an LED.
020 */
021public class RGBWColor {
022    /**
023     * The red component of the color, within [0, 255].
024     */
025    public final int Red;
026    /**
027     * The green component of the color, within [0, 255].
028     */
029    public final int Green;
030    /**
031     * The blue component of the color, within [0, 255].
032     */
033    public final int Blue;
034    /**
035     * The white component of the color, within [0, 255].
036     * Note that not all LED strips support the white component.
037     */
038    public final int White;
039
040    /**
041     * Creates a new RGBW color where all components are off.
042     */
043    public RGBWColor() {
044        this(0, 0, 0, 0);
045    }
046
047    /**
048     * Creates a new RGB color from the given 8-bit components.
049     *
050     * @param red The red component of the color, within [0, 255].
051     * @param green The green component of the color, within [0, 255].
052     * @param blue The blue component of the color, within [0, 255].
053     */
054    public RGBWColor(int red, int green, int blue) {
055        this(red, green, blue, 0);
056    }
057
058    /**
059     * Creates a new RGBW color from the given 8-bit components.
060     *
061     * @param red The red component of the color, within [0, 255].
062     * @param green The green component of the color, within [0, 255].
063     * @param blue The blue component of the color, within [0, 255].
064     * @param white The white component of the color, within [0, 255].
065     *              Note that not all LED strips support the white component.
066     */
067    public RGBWColor(int red, int green, int blue, int white) {
068        Red = red;
069        Green = green;
070        Blue = blue;
071        White = white;
072    }
073
074    /**
075     * Creates a new RGBW color from a WPILib color.
076     * The white component will be left 0.
077     *
078     * @param color The WPILib color
079     */
080    public RGBWColor(Color color) {
081        this(
082            (int)Math.round(color.red * 255.0),
083            (int)Math.round(color.green * 255.0),
084            (int)Math.round(color.blue * 255.0),
085            0
086        );
087    }
088    /**
089     * Creates a new RGBW color from a WPILib 8-bit color.
090     * The white component will be left 0.
091     *
092     * @param color The WPILib color
093     */
094    public RGBWColor(Color8Bit color) {
095        this(color.red, color.green, color.blue, 0);
096    }
097
098    /**
099     * Creates a new RGBW color from the given hex string.
100     *
101     * @param hex The color hex in the form "#RRGGBBWW" or "#RRGGBB".
102     * @return The color if the hex is valid, otherwise {@link Optional#empty()}
103     */
104    public static Optional<RGBWColor> fromHex(String hex) {
105        /* hex string is either 7 (RGB) or 9 (RGBW) characters and starts with # */
106        if ((hex.length() != 7 && hex.length() != 9) || !hex.startsWith("#")) {
107            return Optional.empty();
108        }
109
110        /* {r, g, b, w} */
111        final int[] colors = new int[4];
112        for (int i = 1; i < hex.length(); i += 2) {
113            int upper = hexToNibble(hex.charAt(i));
114            int lower = hexToNibble(hex.charAt(i + 1));
115            if (upper < 0 || lower < 0) {
116                return Optional.empty();
117            }
118            colors[(i - 1) / 2] = (upper << 4) | lower;
119        }
120
121        return Optional.of(new RGBWColor(colors[0], colors[1], colors[2], colors[3]));
122    }
123
124    /**
125     * Creates a new RGBW color from the given HSV color.
126     *
127     * @param h The hue as an angle from [0, 360) deg, where 0 is red.
128     * @param s The saturation as a scalar from [0, 1].
129     * @param v The value as a scalar from [0, 1].
130     * @return The corresponding RGB color; the white component will be 0.
131     */
132    public static RGBWColor fromHSV(double h, double s, double v) {
133        /* wrap h to [0, 360) and clamp s and v */
134        if (h < 0.0 || h >= 360.0) {
135            h -= Math.floor(h / 360.0) * 360.0;
136        }
137        s = Math.max(0.0, Math.min(s, 1.0));
138        v = Math.max(0.0, Math.min(v, 1.0));
139
140        /* range between highest and lowest RGB components */
141        double chroma = s * v;
142        /* 6 regions of hue */
143        double hue_region = h / 60.0;
144
145        /* the highest RGB component */
146        double maxf = v;
147        /* the lowest RGB component */
148        double minf = maxf - chroma;
149
150        /* offset from max/min for the middle RGB component */
151        double Xoffset = chroma * (hue_region - (int)hue_region);
152        /* the middle RGB component; even regions from min, odd from max */
153        double Xf = ((int)hue_region & 1) != 0
154            ? maxf - Xoffset
155            : minf + Xoffset;
156
157        /* all scalars within [0, 1], scale to [0, 255] */
158        int max = (int)Math.round(maxf * 255);
159        int min = (int)Math.round(minf * 255);
160        int X = (int)Math.round(Xf * 255);
161
162        switch ((int)hue_region) {
163            default:
164            case 0:
165                return new RGBWColor(max, X, min);
166            case 1:
167                return new RGBWColor(X, max, min);
168            case 2:
169                return new RGBWColor(min, max, X);
170            case 3:
171                return new RGBWColor(min, X, max);
172            case 4:
173                return new RGBWColor(X, min, max);
174            case 5:
175                return new RGBWColor(max, min, X);
176        }
177    }
178    /**
179     * Creates a new RGBW color from the given HSV color.
180     *
181     * @param h The hue as an angle from [0, 360) deg.
182     * @param s The saturation as a scalar from [0, 1].
183     * @param v The value as a scalar from [0, 1].
184     * @return The corresponding RGB color; the white component will be 0.
185     */
186    public static RGBWColor fromHSV(Angle h, double s, double v) {
187        return fromHSV(h.in(Degrees), s, v);
188    }
189
190    /**
191     * Scales down the components of this color by the given brightness.
192     * <p>
193     * This function returns a new object every call. As a result,
194     * we recommend that this is not called inside a tight loop.
195     *
196     * @param brightness The scalar to apply from [0, 1].
197     * @return New color scaled by the given brightness
198     */
199    public final RGBWColor scaleBrightness(double brightness) {
200        brightness = Math.max(0.0, Math.min(brightness, 1.0));
201        return new RGBWColor(
202            (int)Math.round(Red * brightness),
203            (int)Math.round(Green * brightness),
204            (int)Math.round(Blue * brightness),
205            (int)Math.round(White * brightness)
206        );
207    }
208
209    @Override
210    public boolean equals(Object other) {
211        if (other instanceof RGBWColor color) {
212            return Red == color.Red &&
213                Green == color.Green &&
214                Blue == color.Blue &&
215                White == color.White;
216        }
217        return false;
218    }
219
220    @Override
221    public int hashCode() {
222        return Objects.hash(Red, Green, Blue, White);
223    }
224
225    @Override
226    public String toString() {
227        return "RGBW(" + Red + ", " + Green + ", " + Blue + ", " + White + ")";
228    }
229
230    /**
231     * Returns this RGBW color as a hex string.
232     * @return A hex string in the format "#RRGGBBWW"
233     */
234    public final String toHexString() {
235        StringBuilder hex = new StringBuilder(9);
236        hex.append('#');
237        hex.append(nibbleToHex((Red >> 4) & 0xF));
238        hex.append(nibbleToHex(Red & 0xF));
239        hex.append(nibbleToHex((Green >> 4) & 0xF));
240        hex.append(nibbleToHex(Green & 0xF));
241        hex.append(nibbleToHex((Blue >> 4) & 0xF));
242        hex.append(nibbleToHex(Blue & 0xF));
243        hex.append(nibbleToHex((White >> 4) & 0xF));
244        hex.append(nibbleToHex(White & 0xF));
245        return hex.toString();
246    }
247
248    private static int hexToNibble(char hex) {
249        if ('A' <= hex && hex <= 'F') {
250            return hex - 'A' + 10;
251        } else if ('a' <= hex && hex <= 'f') {
252            return hex - 'a' + 10;
253        } else if ('0' <= hex && hex <= '9') {
254            return hex - '0';
255        } else {
256            /* invalid */
257            return -1;
258        }
259    }
260    private static char nibbleToHex(int nibble) {
261        if (nibble < 10) {
262            return (char)(nibble + '0');
263        } else {
264            return (char)(nibble - 10 + 'A');
265        }
266    }
267}