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.pipeline;
019
020import org.photonvision.common.logging.LogGroup;
021import org.photonvision.common.logging.Logger;
022import org.photonvision.common.util.math.MathUtils;
023
024public class PipelineProfiler {
025    private static boolean shouldLog;
026
027    private static final Logger reflectiveLogger =
028            new Logger(ReflectivePipeline.class, LogGroup.VisionModule);
029    private static final Logger coloredShapeLogger =
030            new Logger(ColoredShapePipeline.class, LogGroup.VisionModule);
031
032    /**
033     * Indices for Reflective profiling 0 - rotateImagePipe 1 - inputCopy (not a pipe) 2 - hsvPipe 3 -
034     * findContoursPipe 4 - speckleRejectPipe 5 - filterContoursPipe 6 - groupContoursPipe 7 -
035     * sortContoursPipe 8 - collect2dTargetsPipe 9 - cornerDetectionPipe 10 - solvePNPPipe (OPTIONAL)
036     * 11 - outputMatPipe (OPTIONAL) 12 - draw2dCrosshairPipe (on input) 13 - draw2dCrosshairPipe (on
037     * output) 14 - draw2dTargetsPipe (on input) 15 - draw2dTargetsPipe (on output) 16 -
038     * draw3dTargetsPipe (OPTIONAL, on input) 17 - draw3dTargetsPipe (OPTIONAL, on output)
039     */
040    private static final String[] ReflectivePipeNames =
041            new String[] {
042                "RotateImage",
043                "HSV",
044                "FindContours",
045                "SpeckleReject",
046                "FilterContours",
047                "GroupContours",
048                "SortContours",
049                "Collect2dTargets",
050                "CornerDetection",
051                "SolvePNP",
052            };
053
054    public static final int ReflectivePipeCount = ReflectivePipeNames.length;
055
056    protected static String getReflectiveProfileString(long[] nanos) {
057        if (nanos.length != ReflectivePipeCount) {
058            return "Invalid data";
059        }
060
061        var sb = new StringBuilder("Profiling - ");
062        double totalMs = 0;
063        for (int i = 0; i < nanos.length; i++) {
064            if ((i == 10 || i == 11 || i == 17 || i == 18) && nanos[i] == 0) {
065                continue; // skip empty pipe profiles
066            }
067
068            sb.append(ReflectivePipeNames[i]);
069            sb.append(": ");
070            var ms = MathUtils.roundTo(nanos[i] / 1e+6, 3);
071            totalMs += ms;
072            sb.append(ms);
073            sb.append("ms");
074            //            boolean isLast = (i + 1 == 17 && nanos[i+1] == 0) || i == 18;
075            //            if (isLast) {
076            //                var foo = "bar";
077            //            } else {
078            sb.append(", ");
079            //            }
080        }
081
082        sb.append("Total: ");
083        sb.append(MathUtils.roundTo(totalMs, 3));
084        sb.append("ms");
085
086        return sb.toString();
087    }
088
089    public static void printReflectiveProfile(long[] nanos) {
090        if (shouldLog) {
091            reflectiveLogger.trace(() -> getReflectiveProfileString(nanos));
092        }
093    }
094
095    public static void enablePrint(boolean enable) {
096        shouldLog = enable;
097    }
098}