allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java

// 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 edu.wpi.first.wpilibj;

import static org.hamcrest.Matchers.both;
import static org.hamcrest.Matchers.greaterThan;
import static org.hamcrest.Matchers.lessThan;
import static org.junit.Assert.assertEquals;
import static org.junit.Assert.assertSame;
import static org.junit.Assert.assertThat;

import edu.wpi.first.wpilibj.test.AbstractComsSetup;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.AtomicLong;
import org.junit.After;
import org.junit.Test;

/**
 * This class should not be run as a test explicitly. Instead it should be extended by tests that
 * use the InterruptableSensorBase.
 */
public abstract class AbstractInterruptTest extends AbstractComsSetup {
  private InterruptableSensorBase m_interruptable = null;

  private InterruptableSensorBase getInterruptable() {
    if (m_interruptable == null) {
      m_interruptable = giveInterruptableSensorBase();
    }
    return m_interruptable;
  }

  @After
  public void interruptTeardown() {
    if (m_interruptable != null) {
      freeInterruptableSensorBase();
      m_interruptable = null;
    }
  }

  /** Give the interruptable sensor base that interrupts can be attached to. */
  abstract InterruptableSensorBase giveInterruptableSensorBase();

  /**
   * Cleans up the interruptable sensor base. This is only called if {@link
   * #giveInterruptableSensorBase()} is called.
   */
  abstract void freeInterruptableSensorBase();

  /** Perform whatever action is required to set the interrupt high. */
  abstract void setInterruptHigh();

  /** Perform whatever action is required to set the interrupt low. */
  abstract void setInterruptLow();

  private class InterruptCounter {
    private final AtomicInteger m_count = new AtomicInteger();

    void increment() {
      m_count.addAndGet(1);
    }

    int getCount() {
      return m_count.get();
    }
  }

  private class TestInterruptHandlerFunction extends InterruptHandlerFunction<InterruptCounter> {
    protected final AtomicBoolean m_exceptionThrown = new AtomicBoolean(false);
    /** Stores the time that the interrupt fires. */
    final AtomicLong m_interruptFireTime = new AtomicLong();
    /** Stores if the interrupt has completed at least once. */
    final AtomicBoolean m_interruptComplete = new AtomicBoolean(false);

    protected Exception m_ex;
    final InterruptCounter m_counter;

    TestInterruptHandlerFunction(InterruptCounter counter) {
      m_counter = counter;
    }

    @Override
    public void interruptFired(int interruptAssertedMask, InterruptCounter param) {
      m_interruptFireTime.set(RobotController.getFPGATime());
      m_counter.increment();
      try {
        // This won't cause the test to fail
        assertSame(m_counter, param);
      } catch (Exception ex) {
        // So we must throw the exception within the test
        m_exceptionThrown.set(true);
        m_ex = ex;
      }
      m_interruptComplete.set(true);
    }

    @Override
    public InterruptCounter overridableParameter() {
      return m_counter;
    }
  }

  @Test(timeout = 1000)
  public void testSingleInterruptsTriggering() throws Exception {
    // Given
    final InterruptCounter counter = new InterruptCounter();
    TestInterruptHandlerFunction function = new TestInterruptHandlerFunction(counter);

    // When
    getInterruptable().requestInterrupts(function);
    getInterruptable().enableInterrupts();

    setInterruptLow();
    Timer.delay(0.01);
    // Note: Utility.getFPGATime() is used because double values can turn over
    // after the robot has been running for a long time
    final long interruptTriggerTime = RobotController.getFPGATime();
    setInterruptHigh();

    // Delay until the interrupt is complete
    while (!function.m_interruptComplete.get()) {
      Timer.delay(0.005);
    }

    // Then
    assertEquals("The interrupt did not fire the expected number of times", 1, counter.getCount());
    // If the test within the interrupt failed
    if (function.m_exceptionThrown.get()) {
      throw function.m_ex;
    }
    final long range = 10000; // in microseconds
    assertThat(
        "The interrupt did not fire within the expected time period (values in milliseconds)",
        function.m_interruptFireTime.get(),
        both(greaterThan(interruptTriggerTime - range))
            .and(lessThan(interruptTriggerTime + range)));
    assertThat(
        "The readRisingTimestamp() did not return the correct value (values in seconds)",
        getInterruptable().readRisingTimestamp(),
        both(greaterThan((interruptTriggerTime - range) / 1e6))
            .and(lessThan((interruptTriggerTime + range) / 1e6)));
  }

  @Test(timeout = 2000)
  public void testMultipleInterruptsTriggering() {
    // Given
    final InterruptCounter counter = new InterruptCounter();
    TestInterruptHandlerFunction function = new TestInterruptHandlerFunction(counter);

    // When
    getInterruptable().requestInterrupts(function);
    getInterruptable().enableInterrupts();

    final int fireCount = 50;
    for (int i = 0; i < fireCount; i++) {
      setInterruptLow();
      setInterruptHigh();
      // Wait for the interrupt to complete before moving on
      while (!function.m_interruptComplete.getAndSet(false)) {
        Timer.delay(0.005);
      }
    }
    // Then
    assertEquals(
        "The interrupt did not fire the expected number of times", fireCount, counter.getCount());
  }

  /** The timeout length for this test in seconds. */
  private static final int synchronousTimeout = 5;

  @Test(timeout = (long) (synchronousTimeout * 1e3))
  public void testSynchronousInterruptsTriggering() {
    // Given
    getInterruptable().requestInterrupts();

    final double synchronousDelay = synchronousTimeout / 2.0;
    final Runnable runnable =
        () -> {
          Timer.delay(synchronousDelay);
          setInterruptLow();
          setInterruptHigh();
        };

    // When

    // Note: the long time value is used because doubles can flip if the robot
    // is left running for long enough
    final long startTimeStamp = RobotController.getFPGATime();
    new Thread(runnable).start();
    // Delay for twice as long as the timeout so the test should fail first
    getInterruptable().waitForInterrupt(synchronousTimeout * 2);
    final long stopTimeStamp = RobotController.getFPGATime();

    // Then
    // The test will not have timed out and:
    final double interruptRunTime = (stopTimeStamp - startTimeStamp) * 1e-6;
    assertEquals(
        "The interrupt did not run for the expected amount of time (units in seconds)",
        synchronousDelay,
        interruptRunTime,
        0.1);
  }

  @Test(timeout = (long) (synchronousTimeout * 1e3))
  public void testSynchronousInterruptsWaitResultTimeout() {
    // Given
    getInterruptable().requestInterrupts();

    // Don't fire interrupt. Expect it to timeout.
    InterruptableSensorBase.WaitResult result =
        getInterruptable().waitForInterrupt(synchronousTimeout / 2);

    assertEquals(
        "The interrupt did not time out correctly.",
        result,
        InterruptableSensorBase.WaitResult.kTimeout);
  }

  @Test(timeout = (long) (synchronousTimeout * 1e3))
  public void testSynchronousInterruptsWaitResultRisingEdge() {
    // Given
    getInterruptable().requestInterrupts();

    final double synchronousDelay = synchronousTimeout / 2.0;
    final Runnable runnable =
        () -> {
          Timer.delay(synchronousDelay);
          setInterruptLow();
          setInterruptHigh();
        };

    new Thread(runnable).start();
    // Delay for twice as long as the timeout so the test should fail first
    InterruptableSensorBase.WaitResult result =
        getInterruptable().waitForInterrupt(synchronousTimeout * 2);

    assertEquals(
        "The interrupt did not fire on the rising edge.",
        result,
        InterruptableSensorBase.WaitResult.kRisingEdge);
  }

  @Test(timeout = (long) (synchronousTimeout * 1e3))
  public void testSynchronousInterruptsWaitResultFallingEdge() {
    // Given
    getInterruptable().requestInterrupts();
    getInterruptable().setUpSourceEdge(false, true);

    final double synchronousDelay = synchronousTimeout / 2.0;
    final Runnable runnable =
        () -> {
          Timer.delay(synchronousDelay);
          setInterruptHigh();
          setInterruptLow();
        };

    new Thread(runnable).start();
    // Delay for twice as long as the timeout so the test should fail first
    InterruptableSensorBase.WaitResult result =
        getInterruptable().waitForInterrupt(synchronousTimeout * 2);

    assertEquals(
        "The interrupt did not fire on the falling edge.",
        result,
        InterruptableSensorBase.WaitResult.kFallingEdge);
  }

  @Test(timeout = 4000)
  public void testDisableStopsInterruptFiring() {
    final InterruptCounter counter = new InterruptCounter();
    TestInterruptHandlerFunction function = new TestInterruptHandlerFunction(counter);

    // When
    getInterruptable().requestInterrupts(function);
    getInterruptable().enableInterrupts();

    final int fireCount = 50;
    for (int i = 0; i < fireCount; i++) {
      setInterruptLow();
      setInterruptHigh();
      // Wait for the interrupt to complete before moving on
      while (!function.m_interruptComplete.getAndSet(false)) {
        Timer.delay(0.005);
      }
    }
    getInterruptable().disableInterrupts();
    // TestBench.out().println("Finished disabling the robot");

    for (int i = 0; i < fireCount; i++) {
      setInterruptLow();
      setInterruptHigh();
      // Just wait because the interrupt should not fire
      Timer.delay(0.005);
    }

    // Then
    assertEquals(
        "The interrupt did not fire the expected number of times", fireCount, counter.getCount());
  }
}