content: /*
   * Copyright (c) 2024 DigitalChickenLabs
   *
   * Permission is hereby granted, free of charge, to any person obtaining a copy
   * of this software and associated documentation files (the "Software"), to deal
   * in the Software without restriction, including without limitation the rights
   * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   * copies of the Software, and to permit persons to whom the Software is
   * furnished to do so, subject to the following conditions:
   *
   * The above copyright notice and this permission notice shall be included in all
   * copies or substantial portions of the Software.
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   * SOFTWARE.
   */
  
  package org.firstinspires.ftc.robotcontroller.external.samples;
  
  import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
  import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  import com.qualcomm.robotcore.hardware.Gamepad;
  
  import org.firstinspires.ftc.robotcore.external.Telemetry;
  
  import java.util.ArrayList;
  import java.util.Stack;
  
  /*
   * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module.
   *
   *   The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs.
   *   Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder.
   *   Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
   *   as can several sonar rangefinders such as the MaxBotix MB1000 series.
   *
   * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
   *
   * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
   *
   * Select, Init and run the "OctoQuad Configuration Tool" OpMode
   * Read the blue User-Interface tips at the top of the telemetry screen.
   * Use the UI buttons to navigate the menu and make any desired changes to the OctoQuad configuration.
   * Use the Program Settings To FLASH option to make any changes permanent.
   *
   * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/
   */
  @TeleOp(name = "OctoQuad Configuration Tool", group="OctoQuad")
  @Disabled
  public class UtilityOctoQuadConfigMenu extends LinearOpMode
  {
      TelemetryMenu.MenuElement rootMenu = new TelemetryMenu.MenuElement("OctoQuad Config Menu", true);
      TelemetryMenu.MenuElement menuHwInfo = new TelemetryMenu.MenuElement("Hardware Information", false);
      TelemetryMenu.EnumOption optionI2cResetMode;
      TelemetryMenu.EnumOption optionChannelBankConfig;
  
      TelemetryMenu.MenuElement menuEncoderDirections = new TelemetryMenu.MenuElement("Set Encoder Directions", false);
      TelemetryMenu.BooleanOption[] optionsEncoderDirections = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS];
  
      TelemetryMenu.MenuElement menuVelocityIntervals = new TelemetryMenu.MenuElement("Velocity Measurement Intervals", false);
      TelemetryMenu.IntegerOption[] optionsVelocityIntervals = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
  
      TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false);
      TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
      TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
  
      TelemetryMenu.OptionElement optionProgramToFlash;
      TelemetryMenu.OptionElement optionSendToRAM;
  
      TelemetryMenu.StaticClickableOption optionExit;
  
      OctoQuad octoquad;
  
      boolean error = false;
  
      @Override
      public void runOpMode()
      {
          octoquad = hardwareMap.getAll(OctoQuad.class).get(0);
  
          if(octoquad.getChipId() != OctoQuad.OCTOQUAD_CHIP_ID)
          {
              telemetry.addLine("Error: cannot communicate with OctoQuad. Check your wiring and configuration and try again");
              telemetry.update();
  
              error = true;
          }
          else
          {
              if(octoquad.getFirmwareVersion().maj != OctoQuad.SUPPORTED_FW_VERSION_MAJ)
              {
                  telemetry.addLine("Error: The OctoQuad is running a different major firmware version than this driver was built for. Cannot run configuration tool");
                  telemetry.update();
  
                  error = true;
              }
          }
  
          if(error)
          {
              waitForStart();
              return;
          }
  
          telemetry.addLine("Retrieving current configuration from OctoQuad");
          telemetry.update();
  
          optionExit = new TelemetryMenu.StaticClickableOption("Exit configuration menu")
          {
              @Override
              void onClick() // called on OpMode thread
              {
                  requestOpModeStop();
              }
          };
  
          optionI2cResetMode = new TelemetryMenu.EnumOption("I2C Reset Mode", OctoQuad.I2cRecoveryMode.values(), octoquad.getI2cRecoveryMode());
          optionChannelBankConfig = new TelemetryMenu.EnumOption("Channel Bank Modes", OctoQuad.ChannelBankConfig.values(), octoquad.getChannelBankConfig());
  
          menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board Firmware: v" + octoquad.getFirmwareVersion()));
          //menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board unique ID: FIXME"));
  
          for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
          {
              optionsEncoderDirections[i] = new TelemetryMenu.BooleanOption(
                      String.format("Encoder %d direction", i),
                      octoquad.getSingleEncoderDirection(i) == OctoQuad.EncoderDirection.REVERSE,
                      "-",
                      "+");
          }
          menuEncoderDirections.addChildren(optionsEncoderDirections);
  
          for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
          {
              optionsVelocityIntervals[i] = new TelemetryMenu.IntegerOption(
                      String.format("Chan %d velocity intvl", i),
                      OctoQuad.MIN_VELOCITY_MEASUREMENT_INTERVAL_MS,
                      OctoQuad.MAX_VELOCITY_MEASUREMENT_INTERVAL_MS,
                      octoquad.getSingleVelocitySampleInterval(i));
          }
          menuVelocityIntervals.addChildren(optionsVelocityIntervals);
  
          for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
          {
              OctoQuad.ChannelPulseWidthParams params = octoquad.getSingleChannelPulseWidthParams(i);
  
              optionsAbsParamsMax[i] = new TelemetryMenu.IntegerOption(
                      String.format("Chan %d max pulse length", i),
                      OctoQuad.MIN_PULSE_WIDTH_US,
                      OctoQuad.MAX_PULSE_WIDTH_US,
                      params.max_length_us);
  
              optionsAbsParamsMin[i] = new TelemetryMenu.IntegerOption(
                      String.format("Chan %d min pulse length", i),
                      OctoQuad.MIN_PULSE_WIDTH_US,
                      OctoQuad.MAX_PULSE_WIDTH_US,
                      params.min_length_us);
          }
          menuAbsParams.addChildren(optionsAbsParamsMin);
          menuAbsParams.addChildren(optionsAbsParamsMax);
  
          optionProgramToFlash = new TelemetryMenu.OptionElement()
          {
              String name = "Program Settings to FLASH";
              long lastClickTime = 0;
  
              @Override
              protected String getDisplayText()
              {
                  if(lastClickTime == 0)
                  {
                      return name;
                  }
                  else
                  {
                      if(System.currentTimeMillis() - lastClickTime < 1000)
                      {
                          return name + " **OK**";
                      }
                      else
                      {
                          lastClickTime = 0;
                          return name;
                      }
                  }
              }
  
              @Override
              void onClick()
              {
                  sendSettingsToRam();
                  octoquad.saveParametersToFlash();
                  lastClickTime = System.currentTimeMillis();
              }
          };
  
          optionSendToRAM = new TelemetryMenu.OptionElement()
          {
              String name = "Send Settings to RAM";
              long lastClickTime = 0;
  
              @Override
              protected String getDisplayText()
              {
                  if(lastClickTime == 0)
                  {
                      return name;
                  }
                  else
                  {
                      if(System.currentTimeMillis() - lastClickTime < 1000)
                      {
                          return name + " **OK**";
                      }
                      else
                      {
                          lastClickTime = 0;
                          return name;
                      }
                  }
              }
  
              @Override
              void onClick()
              {
                  sendSettingsToRam();
                  lastClickTime = System.currentTimeMillis();
              }
          };
  
          rootMenu.addChild(menuHwInfo);
          rootMenu.addChild(optionI2cResetMode);
          rootMenu.addChild(optionChannelBankConfig);
          rootMenu.addChild(menuEncoderDirections);
          rootMenu.addChild(menuVelocityIntervals);
          rootMenu.addChild(menuAbsParams);
          rootMenu.addChild(optionProgramToFlash);
          rootMenu.addChild(optionSendToRAM);
          rootMenu.addChild(optionExit);
  
          TelemetryMenu menu = new TelemetryMenu(telemetry, rootMenu);
  
          while (!isStopRequested())
          {
              menu.loop(gamepad1);
              telemetry.update();
              sleep(20);
          }
      }
  
      void sendSettingsToRam()
      {
          for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
          {
              octoquad.setSingleEncoderDirection(i, optionsEncoderDirections[i].getValue() ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
              octoquad.setSingleVelocitySampleInterval(i, optionsVelocityIntervals[i].getValue());
  
              OctoQuad.ChannelPulseWidthParams params = new OctoQuad.ChannelPulseWidthParams();
              params.max_length_us = optionsAbsParamsMax[i].getValue();
              params.min_length_us = optionsAbsParamsMin[i].getValue();
  
              octoquad.setSingleChannelPulseWidthParams(i, params);
          }
  
          octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue());
          octoquad.setChannelBankConfig((OctoQuad.ChannelBankConfig) optionChannelBankConfig.getValue());
      }
  
      /*
       * Copyright (c) 2023 OpenFTC Team
       *
       * Permission is hereby granted, free of charge, to any person obtaining a copy
       * of this software and associated documentation files (the "Software"), to deal
       * in the Software without restriction, including without limitation the rights
       * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
       * copies of the Software, and to permit persons to whom the Software is
       * furnished to do so, subject to the following conditions:
       *
       * The above copyright notice and this permission notice shall be included in all
       * copies or substantial portions of the Software.
       * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
       * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
       * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
       * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
       * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
       * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
       * SOFTWARE.
       */
  
      public static class TelemetryMenu
      {
          private final MenuElement root;
          private MenuElement currentLevel;
  
          private boolean dpadUpPrev;
          private boolean dpadDnPrev;
          private boolean dpadRightPrev;
          private boolean dpadLeftPrev;
          private boolean xPrev;
          private boolean lbPrev;
  
          private int selectedIdx = 0;
          private Stack<Integer> selectedIdxStack = new Stack<>();
  
          private final Telemetry telemetry;
  
          /**
           * TelemetryMenu constructor
           * @param telemetry pass in 'telemetry' from your OpMode
           * @param root the root menu element
           */
          public TelemetryMenu(Telemetry telemetry, MenuElement root)
          {
              this.root = root;
              this.currentLevel = root;
              this.telemetry = telemetry;
  
              telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
              telemetry.setMsTransmissionInterval(50);
          }
  
          /**
           * Call this from inside your loop to put the current menu state into
           * telemetry, and process gamepad inputs for navigating the menu
           * @param gamepad the gamepad you want to use to navigate the menu
           */
          public void loop(Gamepad gamepad)
          {
              // Capture current state of the gamepad buttons we care about;
              // We can only look once or we risk a race condition
              boolean dpadUp = gamepad.dpad_up;
              boolean dpadDn = gamepad.dpad_down;
              boolean dpadRight = gamepad.dpad_right;
              boolean dpadLeft = gamepad.dpad_left;
              boolean x = gamepad.x;
              boolean lb = gamepad.left_bumper;
  
              // Figure out who our children our at this level
              // and figure out which item is currently highlighted
              // with the selection pointer
              ArrayList<Element> children = currentLevel.children();
              Element currentSelection = children.get(selectedIdx);
  
              // Left and right are inputs to the selected item (if it's an Option)
              if (currentSelection instanceof OptionElement)
              {
                  if (dpadRight && !dpadRightPrev) // rising edge
                  {
                      ((OptionElement) currentSelection).onRightInput();
                  }
                  else if (dpadLeft && !dpadLeftPrev) // rising edge
                  {
                      ((OptionElement) currentSelection).onLeftInput();
                  }
              }
  
              // Up and down navigate the current selection pointer
              if (dpadUp && !dpadUpPrev) // rising edge
              {
                  selectedIdx--; // Move selection pointer up
              }
              else if (dpadDn && !dpadDnPrev) // rising edge
              {
                  selectedIdx++; // Move selection pointer down
              }
  
              // Make selected index sane (don't let it go out of bounds) :eyes:
              if (selectedIdx >= children.size())
              {
                  selectedIdx = children.size()-1;
              }
              else if (selectedIdx < 0)
              {
                  selectedIdx = 0;
              }
  
              // Select: either enter submenu or input to option
              else if (x && !xPrev) // rising edge
              {
                  // Select up element
                  if (currentSelection instanceof SpecialUpElement)
                  {
                      // We can only go up if we're not at the root level
                      if (currentLevel != root)
                      {
                          // Restore selection pointer to where it was before
                          selectedIdx = selectedIdxStack.pop();
  
                          // Change to the parent level
                          currentLevel = currentLevel.parent();
                      }
                  }
                  // Input to option
                  else if (currentSelection instanceof OptionElement)
                  {
                      ((OptionElement) currentSelection).onClick();
                  }
                  // Enter submenu
                  else if (currentSelection instanceof MenuElement)
                  {
                      // Save our current selection pointer so we can restore it
                      // later if the user navigates back up a level
                      selectedIdxStack.push(selectedIdx);
  
                      // We have no idea what's in the submenu :monkey: so best to
                      // just set the selection pointer to the first element
                      selectedIdx = 0;
  
                      // Now the current level becomes the submenu that the selection
                      // pointer was on
                      currentLevel = (MenuElement) currentSelection;
                  }
              }
  
              // Go up a level
              else if (lb && !lbPrev)
              {
                  // We can only go up if we're not at the root level
                  if (currentLevel != root)
                  {
                      // Restore selection pointer to where it was before
                      selectedIdx = selectedIdxStack.pop();
  
                      // Change to the parent level
                      currentLevel = currentLevel.parent();
                  }
              }
  
              // Save the current button states so that we can look for
              // the rising edge the next time around the loop :)
              dpadUpPrev = dpadUp;
              dpadDnPrev = dpadDn;
              dpadRightPrev = dpadRight;
              dpadLeftPrev = dpadLeft;
              xPrev = x;
              lbPrev = lb;
  
              // Start building the text display.
              // First, we add the static directions for gamepad operation
              StringBuilder builder = new StringBuilder();
              builder.append("<font color='#119af5' face=monospace>");
              builder.append("Navigate items.....dpad up/down\n")
                      .append("Select.............X or Square\n")
                      .append("Edit option........dpad left/right\n")
                      .append("Up one level.......left bumper\n");
              builder.append("</font>");
              builder.append("\n");
  
              // Now actually add the menu options. We start by adding the name of the current menu level.
              builder.append("<font face=monospace>");
              builder.append("Current Menu: ").append(currentLevel.name).append("\n");
  
              // Now we loop through all the child elements of this level and add them
              for (int i = 0; i < children.size(); i++)
              {
                  // If the selection pointer is at this index, put a green dot in the box :)
                  if (selectedIdx == i)
                  {
                      builder.append("[<font color=green face=monospace>•</font>] ");
                  }
                  // Otherwise, just put an empty box
                  else
                  {
                      builder.append("[ ] ");
                  }
  
                  // Figure out who the selection pointer is pointing at :eyes:
                  Element e = children.get(i);
  
                  // If it's pointing at a submenu, indicate that it's a submenu to the user
                  // by prefixing "> " to the name.
                  if (e instanceof MenuElement)
                  {
                      builder.append("> ");
                  }
  
                  // Finally, add the element's name
                  builder.append(e.getDisplayText());
  
                  // We musn't forget the newline
                  builder.append("\n");
              }
  
              // Don't forget to close the font tag either
              builder.append("</font>");
  
              // Build the string!!!! :nerd:
              String menu = builder.toString();
  
              // Add it to telemetry
              telemetry.addLine(menu);
          }
  
          public static class MenuElement extends Element
          {
              private String name;
              private ArrayList<Element> children = new ArrayList<>();
  
              /**
               * Create a new MenuElement; may either be the root menu, or a submenu (set isRoot accordingly)
               * @param name the name for this menu
               * @param isRoot whether this is a root menu, or a submenu
               */
              public MenuElement(String name, boolean isRoot)
              {
                  this.name = name;
  
                  // If it's not the root menu, we add the up one level option as the first element
                  if (!isRoot)
                  {
                      children.add(new SpecialUpElement());
                  }
              }
  
              /**
               * Add a child element to this menu (may either be an Option or another menu)
               * @param child the child element to add
               */
              public void addChild(Element child)
              {
                  child.setParent(this);
                  children.add(child);
              }
  
              /**
               * Add multiple child elements to this menu (may either be option, or another menu)
               * @param children the children to add
               */
              public void addChildren(Element[] children)
              {
                  for (Element e : children)
                  {
                      e.setParent(this);
                      this.children.add(e);
                  }
              }
  
              @Override
              protected String getDisplayText()
              {
                  return name;
              }
  
              private ArrayList<Element> children()
              {
                  return children;
              }
          }
  
          public static abstract class OptionElement extends Element
          {
              /**
               * Override this to get notified when the element is clicked
               */
              void onClick() {}
  
              /**
               * Override this to get notified when the element gets a "left edit" input
               */
              protected void onLeftInput() {}
  
              /**
               * Override this to get notified when the element gets a "right edit" input
               */
              protected void onRightInput() {}
          }
  
          public static class EnumOption extends OptionElement
          {
              protected int idx = 0;
              protected Enum[] e;
              protected String name;
  
              public EnumOption(String name, Enum[] e)
              {
                  this.e = e;
                  this.name = name;
              }
  
              public EnumOption(String name, Enum[] e, Enum def)
              {
                  this(name, e);
                  idx = def.ordinal();
              }
  
              @Override
              public void onLeftInput()
              {
                  idx++;
  
                  if(idx > e.length-1)
                  {
                      idx = 0;
                  }
              }
  
              @Override
              public void onRightInput()
              {
                  idx--;
  
                  if(idx < 0)
                  {
                      idx = e.length-1;
                  }
              }
  
              @Override
              public void onClick()
              {
                  //onRightInput();
              }
  
              @Override
              protected String getDisplayText()
              {
                  return String.format("%s: <font color='#e37c07' face=monospace>%s</font>", name, e[idx].name());
              }
  
              public Enum getValue()
              {
                  return e[idx];
              }
          }
  
          public static class IntegerOption extends OptionElement
          {
              protected int i;
              protected int min;
              protected int max;
              protected String name;
  
              public IntegerOption(String name, int min, int max, int def)
              {
                  this.name = name;
                  this.min = min;
                  this.max = max;
                  this.i = def;
              }
  
              @Override
              public void onLeftInput()
              {
                  i--;
  
                  if(i < min)
                  {
                      i = max;
                  }
              }
  
              @Override
              public void onRightInput()
              {
                  i++;
  
                  if(i > max)
                  {
                      i = min;
                  }
              }
  
              @Override
              public void onClick()
              {
                  //onRightInput();
              }
  
              @Override
              protected String getDisplayText()
              {
                  return String.format("%s: <font color='#e37c07' face=monospace>%d</font>", name, i);
              }
  
              public int getValue()
              {
                  return i;
              }
          }
  
          static class BooleanOption extends OptionElement
          {
              private String name;
              private boolean val = true;
  
              private String customTrue;
              private String customFalse;
  
              BooleanOption(String name, boolean def)
              {
                  this.name = name;
                  this.val = def;
              }
  
              BooleanOption(String name, boolean def, String customTrue, String customFalse)
              {
                  this(name, def);
                  this.customTrue = customTrue;
                  this.customFalse = customFalse;
              }
  
              @Override
              public void onLeftInput()
              {
                  val = !val;
              }
  
              @Override
              public void onRightInput()
              {
                  val = !val;
              }
  
              @Override
              public void onClick()
              {
                  val = !val;
              }
  
              @Override
              protected String getDisplayText()
              {
                  String valStr;
  
                  if(customTrue != null && customFalse != null)
                  {
                      valStr = val ? customTrue : customFalse;
                  }
                  else
                  {
                      valStr = val ? "true" : "false";
                  }
  
                  return String.format("%s: <font color='#e37c07' face=monospace>%s</font>", name, valStr);
              }
  
              public boolean getValue()
              {
                  return val;
              }
          }
  
          /**
           *
           */
          public static class StaticItem extends OptionElement
          {
              private String name;
  
              public StaticItem(String name)
              {
                  this.name = name;
              }
  
              @Override
              protected String getDisplayText()
              {
                  return name;
              }
          }
  
          public static abstract class StaticClickableOption extends OptionElement
          {
              private String name;
  
              public StaticClickableOption(String name)
              {
                  this.name = name;
              }
  
              abstract void onClick();
  
              @Override
              protected String getDisplayText()
              {
                  return name;
              }
          }
  
          private static abstract class Element
          {
              private MenuElement parent;
  
              protected void setParent(MenuElement parent)
              {
                  this.parent = parent;
              }
  
              protected MenuElement parent()
              {
                  return parent;
              }
  
              protected abstract String getDisplayText();
          }
  
          private static class SpecialUpElement extends Element
          {
              @Override
              protected String getDisplayText()
              {
                  return "<font color='#119af5' face=monospace>.. ↰ Up One Level</font>";
              }
          }
      }
  }
  