// 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 static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.wpilib.units.Units.InchesPerSecond;
import static org.wpilib.units.Units.RPM;

import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Twist2d;

class ChassisVelocitiesTest {
  private static final double kEpsilon = 1E-9;

  @Test
  void testDiscretize() {
    final var target = new ChassisVelocities(1.0, 0.0, 0.5);
    final var duration = 1.0;
    final var dt = 0.01;

    final var velocities = target.discretize(duration);
    final var twist = new Twist2d(velocities.vx * dt, velocities.vy * dt, velocities.omega * dt);

    var pose = Pose2d.kZero;
    for (double time = 0; time < duration; time += dt) {
      pose = pose.plus(twist.exp());
    }

    final var result = pose; // For lambda capture
    assertAll(
        () -> assertEquals(target.vx * duration, result.getX(), kEpsilon),
        () -> assertEquals(target.vy * duration, result.getY(), kEpsilon),
        () -> assertEquals(target.omega * duration, result.getRotation().getRadians(), kEpsilon));
  }

  @Test
  void testMeasureConstructor() {
    var vx = InchesPerSecond.of(14.52);
    var vy = InchesPerSecond.of(0);
    var omega = RPM.of(0.02);
    var velocities = new ChassisVelocities(vx, vy, omega);

    assertAll(
        () -> assertEquals(0.368808, velocities.vx, kEpsilon),
        () -> assertEquals(0, velocities.vy, kEpsilon),
        () -> assertEquals(0.002094395102, velocities.omega, kEpsilon));
  }

  @Test
  void testToRobotRelative() {
    final var chassisVelocities =
        new ChassisVelocities(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);

    assertAll(
        () -> assertEquals(0.0, chassisVelocities.vx, kEpsilon),
        () -> assertEquals(1.0, chassisVelocities.vy, kEpsilon),
        () -> assertEquals(0.5, chassisVelocities.omega, kEpsilon));
  }

  @Test
  void testToFieldRelative() {
    final var chassisVelocities =
        new ChassisVelocities(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));

    assertAll(
        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisVelocities.vx, kEpsilon),
        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisVelocities.vy, kEpsilon),
        () -> assertEquals(0.5, chassisVelocities.omega, kEpsilon));
  }

  @Test
  void testPlus() {
    final var left = new ChassisVelocities(1.0, 0.5, 0.75);
    final var right = new ChassisVelocities(2.0, 1.5, 0.25);

    final var chassisVelocities = left.plus(right);

    assertAll(
        () -> assertEquals(3.0, chassisVelocities.vx),
        () -> assertEquals(2.0, chassisVelocities.vy),
        () -> assertEquals(1.0, chassisVelocities.omega));
  }

  @Test
  void testMinus() {
    final var left = new ChassisVelocities(1.0, 0.5, 0.75);
    final var right = new ChassisVelocities(2.0, 0.5, 0.25);

    final var chassisVelocities = left.minus(right);

    assertAll(
        () -> assertEquals(-1.0, chassisVelocities.vx),
        () -> assertEquals(0.0, chassisVelocities.vy),
        () -> assertEquals(0.5, chassisVelocities.omega));
  }

  @Test
  void testUnaryMinus() {
    final var chassisVelocities = (new ChassisVelocities(1.0, 0.5, 0.75)).unaryMinus();

    assertAll(
        () -> assertEquals(-1.0, chassisVelocities.vx),
        () -> assertEquals(-0.5, chassisVelocities.vy),
        () -> assertEquals(-0.75, chassisVelocities.omega));
  }

  @Test
  void testMultiplication() {
    final var chassisVelocities = (new ChassisVelocities(1.0, 0.5, 0.75)).times(2.0);

    assertAll(
        () -> assertEquals(2.0, chassisVelocities.vx),
        () -> assertEquals(1.0, chassisVelocities.vy),
        () -> assertEquals(1.5, chassisVelocities.omega));
  }

  @Test
  void testDivision() {
    final var chassisVelocities = (new ChassisVelocities(1.0, 0.5, 0.75)).div(2.0);

    assertAll(
        () -> assertEquals(0.5, chassisVelocities.vx),
        () -> assertEquals(0.25, chassisVelocities.vy),
        () -> assertEquals(0.375, chassisVelocities.omega));
  }

  @Test
  void testInterpolation() {
    var start = new ChassisVelocities(0.0, 0.0, 0.0);
    var end = new ChassisVelocities(10.0, 20.0, 30.0);
    var result = start.interpolate(end, 0.5);

    assertAll(
        () -> assertEquals(5.0, result.vx, kEpsilon),
        () -> assertEquals(10.0, result.vy, kEpsilon),
        () -> assertEquals(15.0, result.omega, kEpsilon));
  }

  @Test
  void testInterpolationAtBounds() {
    var start = new ChassisVelocities(1.0, 2.0, 3.0);
    var end = new ChassisVelocities(4.0, 5.0, 6.0);

    // Test t = 0 (should return start)
    var resultStart = start.interpolate(end, 0.0);
    assertAll(
        () -> assertEquals(1.0, resultStart.vx, kEpsilon),
        () -> assertEquals(2.0, resultStart.vy, kEpsilon),
        () -> assertEquals(3.0, resultStart.omega, kEpsilon));

    // Test t = 1 (should return end)
    var resultEnd = start.interpolate(end, 1.0);
    assertAll(
        () -> assertEquals(4.0, resultEnd.vx, kEpsilon),
        () -> assertEquals(5.0, resultEnd.vy, kEpsilon),
        () -> assertEquals(6.0, resultEnd.omega, kEpsilon));
  }
}
