001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.kinematics;
006
007import java.util.Arrays;
008import java.util.Objects;
009import java.util.function.BinaryOperator;
010
011public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
012  public SwerveModulePosition[] positions;
013
014  /**
015   * Creates a new SwerveDriveWheelPositions instance.
016   *
017   * @param positions The swerve module positions. This will be deeply copied.
018   */
019  public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
020    this.positions = new SwerveModulePosition[positions.length];
021    for (int i = 0; i < positions.length; i++) {
022      this.positions[i] = positions[i].copy();
023    }
024  }
025
026  @Override
027  public boolean equals(Object obj) {
028    if (obj instanceof SwerveDriveWheelPositions) {
029      SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
030      return Arrays.equals(this.positions, other.positions);
031    }
032    return false;
033  }
034
035  @Override
036  public int hashCode() {
037    // Cast to interpret positions as single argument, not array of the arguments
038    return Objects.hash((Object) positions);
039  }
040
041  @Override
042  public String toString() {
043    return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
044  }
045
046  private SwerveDriveWheelPositions generate(
047      SwerveDriveWheelPositions other, BinaryOperator<SwerveModulePosition> generator) {
048    if (other.positions.length != positions.length) {
049      throw new IllegalArgumentException("Inconsistent number of modules!");
050    }
051    var newPositions = new SwerveModulePosition[positions.length];
052    for (int i = 0; i < positions.length; i++) {
053      newPositions[i] = generator.apply(positions[i], other.positions[i]);
054    }
055    return new SwerveDriveWheelPositions(newPositions);
056  }
057
058  @Override
059  public SwerveDriveWheelPositions copy() {
060    return new SwerveDriveWheelPositions(positions);
061  }
062
063  @Override
064  public SwerveDriveWheelPositions minus(SwerveDriveWheelPositions other) {
065    return generate(other, (a, b) -> a.minus(b));
066  }
067
068  @Override
069  public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
070    return generate(endValue, (a, b) -> a.interpolate(b, t));
071  }
072}