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.hardware.digitalchickenlabs.OctoQuadBase;
  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  import com.qualcomm.robotcore.util.ElapsedTime;
  import com.qualcomm.robotcore.util.MovingStatistics;
  import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  
  
  import org.firstinspires.ftc.robotcore.external.Telemetry;
  import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  
  import java.util.ArrayList;
  import java.util.List;
  
  /*
   * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module
   *
   * The OctoQuad has 8 input channels that can used to read either Quadrature Encoder signals (like with most FTC motors)
   * or Pulse-Width style Absolute Encoder inputs (eg: REV Through Bore encoder pulse width output).
   *
   * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width measurement and velocity measurement.
   * For a more basic OpMode just showing how to read encoder inputs, see the SensorOctoQuad sample.
   *
   * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
   *
   * One system that requires a lot of encoder inputs is a Swerve Drive system.
   * Such a drive requires two encoders per drive module: one for position & velocity of the Drive motor/wheel, and one for position/angle of the Steering motor.
   * The Drive motor usually requires a quadrature encoder, and the Steering motor requires an Absolute encoder.
   * This quantity and combination of encoder inputs is a challenge, but a single OctoQuad could be used to read them all.
   *
   * This sample will configure an OctoQuad for a swerve drive, as follows
   *  - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs
   *  - Configure a velocity sample interval of 25 mSec
   *  - Configure a pulse width input range of 1-1024 uSec for a REV Through Bore Encoder's Absolute Pulse output.
   *
   * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage the 4 swerve modules.
   * An OctoSwerveModule class will be created to configure and read a single swerve module.
   *
   * Wiring:
   *  The OctoQuad will be configured to accept Quadrature encoders on the first four channels and Absolute (pulse width) encoders on the last four channels.
   *
   *  The standard 4-pin to 4-pin cable can be used to connect each Driver Motor encoder to the OctoQuad. (channels 0-3)
   *
   *  A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder to the OctoQuad. (channels 4-7)
   *  To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the Provided 6-pin to 4-pin cable will need to be modified.
   *    At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the ABS pin-5 position. This can be done easily
   *    by using a small flathead screwdriver to lift the small white tab holding the metal wire crimp in place and gently pulling the wire out.
   *  See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment.
   *
   * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
   * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
   *
   * Note: If you prefer, you can move the two support classes from this file, and place them in their own files.
   *       But leaving them in place is simpler for this example.
   *
   * See the sensor's product page: https://www.tindie.com/products/35114/
   */
  @TeleOp(name="OctoQuad Advanced", group="OctoQuad")
  @Disabled
  public class SensorOctoQuadAdv extends LinearOpMode {
  
      @Override
      public void runOpMode() throws InterruptedException {
          // Connect to the OctoQuad by looking up its name in the hardwareMap.
          OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
  
          // Create the interface for the Swerve Drive Encoders
          OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad);
  
          // Display the OctoQuad firmware revision
          telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion());
          telemetry.addLine("\nPress START to read encoder values");
          telemetry.update();
  
          waitForStart();
  
          // Configure the telemetry for optimal display of data.
          telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
          telemetry.setMsTransmissionInterval(50);
  
          // Run stats to determine cycle times.
          MovingStatistics avgTime = new MovingStatistics(100);
          ElapsedTime elapsedTime = new ElapsedTime();
  
          while (opModeIsActive()) {
              telemetry.addData(">", "Press X to Reset Encoders\n");
  
              if(gamepad1.x) {
                  octoquad.resetAllPositions();
              }
  
              // read all the swerve drive encoders and update positions and velocities.
              octoSwerveDrive.updateModules();
              octoSwerveDrive.show(telemetry);
  
              // Update cycle time stats
              avgTime.add(elapsedTime.nanoseconds());
              elapsedTime.reset();
  
              telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000);
              telemetry.update();
          }
      }
  }
  
  // ============================  Internal (Inner) Classes  =============================
  
  /***
   * OctoSwerveDrive class manages 4 Swerve Modules
   * - Performs general OctoQuad initialization
   * - Creates 4 module classes, one for each swerve module
   * - Updates swerve drive status by reading all data from OctoQuad and Updating each module
   * - Displays all swerve drive data as telemetry
   */
  class OctoSwerveDrive {
  
      private final OctoQuad octoquad;
      private final List<OctoSwerveModule> allModules = new ArrayList<>();
  
      // members to hold encoder data for each module.
      public final OctoSwerveModule LeftFront;
      public final OctoSwerveModule RightFront;
      public final OctoSwerveModule LeftBack;
      public final OctoSwerveModule RightBack;
  
      // Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel)
      private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock();
  
      public OctoSwerveDrive(OctoQuad octoquad) {
          this.octoquad = octoquad;
  
          // Clear out all prior settings and encoder data before setting up desired configuration
          octoquad.resetEverything();
  
          // Assume first 4 channels are relative encoders and the next 4 are absolute encoders
          octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH);
  
          // Create the four OctoSwerveModules, and add them to a 'list' for easier reference.
  
          // Note: The wheel/channel order is set here.  Ensure your encoder cables match.
          //
          // Note: The angleOffset must be set for each module so a forward facing wheel shows a steer angle of 0 degrees.
          //       The process for determining the correct angleOffsets is as follows:
          //        1) Set all the angleValues (below) to zero then build and deploy the code.
          //        2) Physically rotate all the swerve drives so the wheels are facing forward in the desired 0 degree position
          //        3) Run the OpMode, view the telemetry and record the "Degrees" value for each module.
          //        4) Update the code by entering the recorded Degrees value for each module as the angleOffset (last) parameter in the lines below.
          //
          // Rebuild and deploy the new code.  Verify that the telemetry now indicates 0 degrees when the wheels are facing forward.
          // Also verify that the correct module values change appropriately when you manually spin (drive) and rotate (steer) a wheel.
  
          allModules.add(LeftFront  = new OctoSwerveModule(octoquad, "LF ",0,0));//  Drive = 0, Steer = 4
          allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));//  Drive = 1, Steer = 5
          allModules.add(LeftBack   = new OctoSwerveModule(octoquad, "LB ",2,0));//  Drive = 2, Steer = 6
          allModules.add(RightBack  = new OctoSwerveModule(octoquad, "RB ",3,0));//  Drive = 3, Steer = 7
  
          // now make sure the settings persist through any power glitches.
          octoquad.saveParametersToFlash();
      }
  
      /**
       * Updates all 4 swerve modules
       */
      public void updateModules() {
          // Read full OctoQuad data block and then pass DataBlock to each swerve module to update themselves.
          octoquad.readAllEncoderData(encoderDataBlock);
          for (OctoSwerveModule module : allModules) {
              module.updateModule(encoderDataBlock);
          }
      }
  
      /**
       * Generate telemetry data for all modules.
       * @param telemetry OpMode Telemetry object
       */
      public void show(Telemetry telemetry) {
          //  create general header block and then have each module add its own telemetry
          telemetry.addData("pos", "   Count     CPS  Degree    DPS");
          for (OctoSwerveModule module : allModules) {
              module.show(telemetry);
          }
      }
  }
  
  /***
   * The OctoSwerveModule class manages a single swerve module
   */
  class OctoSwerveModule {
  
      public  double driveCounts;
      public  double driveCountsPerSec;
      public  double steerDegrees;
      public  double steerDegreesPerSec;
  
      private final String   name;
      private final int      channel;
      private final double   angleOffset;
      private final double   steerDirMult;
  
      private static final int    VELOCITY_SAMPLE_INTERVAL_MS = 25;   // To provide 40 updates per second.
      private static final double DEGREES_PER_US = (360.0 / 1024.0);  // based on REV Through Bore Encoder
      private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS);
  
      // The correct drive and turn directions must be set for the Swerve Module based on the specific hardware geometry.
      // Forward motion must generate an increasing drive count.
      // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees)
      private static final boolean INVERT_DRIVE_ENCODER = false; // Set true if forward motion decreases drive "Count"
      private static final boolean INVERT_STEER_ENCODER = false; // Set true if counter clockwise steer action decreases steer "Degree"
  
      /***
       * @param octoquad provide access to configure OctoQuad
       * @param name name used for telemetry display
       * @param quadChannel Quadrature encoder channel.  Pulse Width channel is this + 4
       * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. (see comments above)
       */
      public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) {
          this.name = name;
          this.channel = quadChannel;
          this.angleOffset = angleOffset;
          this.steerDirMult = INVERT_STEER_ENCODER  ? -1 : 1 ;  // create a multiplier to flip the steer angle.
  
          // Set the drive encoder direction.  Note the absolute encoder does not have built-in direction inversion.
          octoquad.setSingleEncoderDirection(channel, INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
  
          // Set the velocity sample interval on both encoders
          octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS);
          octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS);
  
          // Setup Absolute encoder pulse range to match REV Through Bore encoder.
          octoquad.setSingleChannelPulseWidthParams (channel + 4, new OctoQuad.ChannelPulseWidthParams(1,1024));
      }
  
      /***
       * Calculate the Swerve module's position and velocity values
       * @param encoderDataBlock  most recent full data block read from OctoQuad.
       */
      public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) {
          driveCounts = encoderDataBlock.positions[channel];  // get Counts.
          driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert counts/interval to counts/sec
  
          // convert uS to degrees.  Add in any possible direction flip.
          steerDegrees = AngleUnit.normalizeDegrees((encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US * steerDirMult) - angleOffset);
          // convert uS/interval to deg per sec.  Add in any possible direction flip.
          steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * DEGREES_PER_US * steerDirMult * VELOCITY_SAMPLES_PER_S;
      }
  
      /**
       * Display the Swerve module's state as telemetry
       * @param telemetry OpMode Telemetry object
       */
      public void show(Telemetry telemetry) {
          telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec);
      }
  }
  