// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.wpilib.math.kinematics;

import org.wpilib.math.geometry.Twist2d;
import org.wpilib.math.interpolation.Interpolator;

/**
 * Helper class that converts a chassis velocity (dx and dtheta components) into wheel velocities
 * and chassis accelerations into wheel accelerations. Robot code should not use this directly-
 * Instead, use the particular type for your drivetrain (e.g., {@link DifferentialDriveKinematics}).
 *
 * @param <P> The type of the wheel positions.
 * @param <S> The type of the wheel velocities.
 * @param <A> The type of the wheel accelerations.
 */
public interface Kinematics<P, S, A> extends Interpolator<P> {
  /**
   * Performs forward kinematics to return the resulting chassis velocity from the wheel velocities.
   * This method is often used for odometry -- determining the robot's position on the field using
   * data from the real-world velocity of each wheel on the robot.
   *
   * @param wheelVelocities The velocities of the wheels.
   * @return The chassis velocity.
   */
  ChassisVelocities toChassisVelocities(S wheelVelocities);

  /**
   * Performs inverse kinematics to return the wheel velocities from a desired chassis velocity.
   * This method is often used to convert joystick values into wheel velocities.
   *
   * @param chassisVelocities The desired chassis velocity.
   * @return The wheel velocities.
   */
  S toWheelVelocities(ChassisVelocities chassisVelocities);

  /**
   * Performs forward kinematics to return the resulting chassis accelerations from the wheel
   * accelerations. This method is often used for dynamics calculations -- determining the robot's
   * acceleration on the field using data from the real-world acceleration of each wheel on the
   * robot.
   *
   * @param wheelAccelerations The accelerations of the wheels.
   * @return The chassis accelerations.
   */
  ChassisAccelerations toChassisAccelerations(A wheelAccelerations);

  /**
   * Performs inverse kinematics to return the wheel accelerations from a desired chassis
   * acceleration. This method is often used for dynamics calculations -- converting desired robot
   * accelerations into individual wheel accelerations.
   *
   * @param chassisAccelerations The desired chassis accelerations.
   * @return The wheel accelerations.
   */
  A toWheelAccelerations(ChassisAccelerations chassisAccelerations);

  /**
   * Performs forward kinematics to return the resulting Twist2d from the given change in wheel
   * positions. This method is often used for odometry -- determining the robot's position on the
   * field using changes in the distance driven by each wheel on the robot.
   *
   * @param start The starting distances driven by the wheels.
   * @param end The ending distances driven by the wheels.
   * @return The resulting Twist2d in the robot's movement.
   */
  Twist2d toTwist2d(P start, P end);

  /**
   * Returns a copy of the wheel positions object.
   *
   * @param positions The wheel positions object to copy.
   * @return A copy.
   */
  P copy(P positions);

  /**
   * Copies the value of the wheel positions object into the output.
   *
   * @param positions The wheel positions object to copy. Will not be modified.
   * @param output The output variable of the copy operation. Will have the same value as the
   *     positions object after the call.
   */
  void copyInto(P positions, P output);
}
