// 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.hardware.expansionhub;

import java.util.OptionalInt;
import org.wpilib.framework.RobotBase;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.networktables.BooleanSubscriber;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.system.SystemServer;
import org.wpilib.system.Timer;

/** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */
public class ExpansionHub implements AutoCloseable {
  private static class DataStore implements AutoCloseable {
    private final int m_usbId;
    private int m_refCount;
    private int m_reservedMotorMask;
    private int m_reservedServoMask;
    private final Object m_reserveLock = new Object();

    private final OptionalInt[] followerConfiguration = new OptionalInt[4];
    private final int[] followerVisited = new int[4];

    private final BooleanSubscriber m_hubConnectedSubscriber;

    DataStore(int usbId) {
      m_usbId = usbId;
      m_storeMap[usbId] = this;

      NetworkTableInstance systemServer = SystemServer.getSystemServer();

      m_hubConnectedSubscriber =
          systemServer.getBooleanTopic("/rhsp/" + usbId + "/connected").subscribe(false);

      for (int i = 0; i < followerConfiguration.length; i++) {
        followerConfiguration[i] = OptionalInt.empty();
      }

      // Wait up to half a second for connected to come up, using a poll loop to
      // ensure we don't block.
      if (RobotBase.isReal()) {
        double startTime = Timer.getMonotonicTimestamp();
        while (Timer.getMonotonicTimestamp() - startTime < 0.5) {
          if (m_hubConnectedSubscriber.get(false)) {
            break;
          }
          Timer.delay(0.01);
        }
      }
    }

    @Override
    public void close() {
      m_storeMap[m_usbId] = null;
    }

    public void addRef() {
      m_refCount++;
    }

    public void removeRef() {
      m_refCount--;
      if (m_refCount == 0) {
        this.close();
      }
    }
  }

  private static final DataStore[] m_storeMap = new DataStore[4];

  private static void checkUsbId(int usbId) {
    if (usbId < 0 || usbId > 3) {
      throw new IllegalArgumentException("USB Port " + usbId + " out of range");
    }
  }

  private static DataStore getForUsbId(int usbId) {
    checkUsbId(usbId);
    synchronized (m_storeMap) {
      DataStore store = m_storeMap[usbId];
      if (store == null) {
        store = new DataStore(usbId);
      }
      store.addRef();
      return store;
    }
  }

  private static void freeHub(DataStore store) {
    synchronized (m_storeMap) {
      store.removeRef();
    }
  }

  private final DataStore m_dataStore;

  /**
   * Constructs a new ExpansionHub for a given USB ID.
   *
   * <p>Multiple instances can be constructed, but will point to the same backing object with a ref
   * count.
   *
   * @param usbId The USB Port ID the hub is plugged into.
   */
  public ExpansionHub(int usbId) {
    m_dataStore = getForUsbId(usbId);
  }

  /**
   * Closes an ExpansionHub object. Will not close any other instances until the last instance is
   * closed.
   */
  @Override
  public void close() {
    freeHub(m_dataStore);
  }

  boolean checkServoChannel(int channel) {
    return channel >= 0 && channel < 6;
  }

  boolean checkAndReserveServo(int channel) {
    int mask = 1 << channel;
    synchronized (m_dataStore.m_reserveLock) {
      if ((m_dataStore.m_reservedServoMask & mask) != 0) {
        return false;
      }
      m_dataStore.m_reservedServoMask |= mask;
      return true;
    }
  }

  void unreserveServo(int channel) {
    int mask = 1 << channel;
    synchronized (m_dataStore.m_reserveLock) {
      m_dataStore.m_reservedServoMask &= ~mask;
    }
  }

  boolean checkMotorChannel(int channel) {
    return channel >= 0 && channel < 4;
  }

  boolean checkAndReserveMotor(int channel) {
    int mask = 1 << channel;
    synchronized (m_dataStore.m_reserveLock) {
      if ((m_dataStore.m_reservedMotorMask & mask) != 0) {
        return false;
      }
      m_dataStore.m_reservedMotorMask |= mask;
      return true;
    }
  }

  void unreserveMotor(int channel) {
    int mask = 1 << channel;
    synchronized (m_dataStore.m_reserveLock) {
      m_dataStore.followerConfiguration[channel] = OptionalInt.empty();
      m_dataStore.m_reservedMotorMask &= ~mask;
    }
  }

  void reportUsage(String device, String data) {
    HAL.reportUsage("ExpansionHub[" + m_dataStore.m_usbId + "]/" + device, data);
  }

  /**
   * Constructs a servo at the requested channel on this hub.
   *
   * <p>Only a single instance of each servo per hub can be constructed at a time.
   *
   * @param channel The servo channel
   * @return Servo object
   */
  public ExpansionHubServo makeServo(int channel) {
    return new ExpansionHubServo(m_dataStore.m_usbId, channel);
  }

  /**
   * Constructs a continuous rotation servo at the requested channel on this hub.
   *
   * <p>Only a single instance of each servo per hub can be constructed at a time.
   *
   * @param channel The servo channel
   * @return Continuous rotation servo object
   */
  public ExpansionHubCRServo makeCRServo(int channel) {
    return new ExpansionHubCRServo(m_dataStore.m_usbId, channel);
  }

  /**
   * Constructs a motor at the requested channel on this hub.
   *
   * <p>Only a single instance of each motor per hub can be constructed at a time.
   *
   * @param channel The motor channel
   * @return Motor object
   */
  public ExpansionHubMotor makeMotor(int channel) {
    return new ExpansionHubMotor(m_dataStore.m_usbId, channel);
  }

  /**
   * Gets if the hub is currently connected over USB.
   *
   * @return True if hub connection, otherwise false
   */
  public boolean isHubConnected() {
    return m_dataStore.m_hubConnectedSubscriber.get(false);
  }

  private String getFollowerStringCycle(int baseChannel, int[] followerVisited) {
    StringBuilder sb = new StringBuilder();
    sb.append(baseChannel);
    int current = baseChannel;
    while (followerVisited[current] != baseChannel) {
      current = followerVisited[current];
      sb.append(" -> ").append(current);
    }
    sb.append(" -> ").append(followerVisited[current]);
    return sb.toString();
  }

  private void validateRootFollower(int baseChannel, int channel, int[] followerVisited) {
    if (followerVisited[channel] != -1) {
      throw new IllegalStateException(
          "Follower cycle detected on hub "
              + m_dataStore.m_usbId
              + ": "
              + getFollowerStringCycle(baseChannel, followerVisited));
    }
    OptionalInt leader = m_dataStore.followerConfiguration[channel];
    if (leader.isEmpty()) {
      return;
    }
    followerVisited[channel] = leader.getAsInt();
    validateRootFollower(baseChannel, leader.getAsInt(), followerVisited);
  }

  private void validateFollowerConfiguration() {
    for (int i = 0; i < m_dataStore.followerConfiguration.length; i++) {
      for (int j = 0; j < m_dataStore.followerVisited.length; j++) {
        m_dataStore.followerVisited[j] = -1;
      }
      validateRootFollower(i, i, m_dataStore.followerVisited);
    }
  }

  void addFollower(int leaderChannel, int followerChannel) {
    synchronized (m_dataStore.m_reserveLock) {
      m_dataStore.followerConfiguration[followerChannel] = OptionalInt.of(leaderChannel);
      validateFollowerConfiguration();
    }
  }

  void removeFollower(int followerChannel) {
    synchronized (m_dataStore.m_reserveLock) {
      m_dataStore.followerConfiguration[followerChannel] = OptionalInt.empty();
    }
  }

  /**
   * Gets the USB ID of this hub.
   *
   * @return The USB ID
   */
  public int getUsbId() {
    return m_dataStore.m_usbId;
  }
}
