allwpilib/hal/src/main/native/sim/mockdata/SimDeviceData.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "mockdata/SimDeviceData.h"  // NOLINT(build/include_order)

#include <algorithm>

#include "SimDeviceDataInternal.h"

using namespace hal;

namespace hal {
namespace init {
void InitializeSimDeviceData() {
  static SimDeviceData sdd;
  ::hal::SimSimDeviceData = &sdd;
}
}  // namespace init
}  // namespace hal

SimDeviceData* hal::SimSimDeviceData;

SimDeviceData::Device* SimDeviceData::LookupDevice(HAL_SimDeviceHandle handle) {
  if (handle <= 0) return nullptr;
  --handle;
  if (static_cast<uint32_t>(handle) >= m_devices.size() || !m_devices[handle])
    return nullptr;
  return m_devices[handle].get();
}

SimDeviceData::Value* SimDeviceData::LookupValue(HAL_SimValueHandle handle) {
  if (handle <= 0) return nullptr;

  // look up device
  Device* deviceImpl = LookupDevice(handle >> 16);

  // look up value
  handle &= 0xffff;
  --handle;
  if (static_cast<uint32_t>(handle) >= deviceImpl->values.size() ||
      !deviceImpl->values[handle])
    return nullptr;

  return deviceImpl->values[handle].get();
}

HAL_SimDeviceHandle SimDeviceData::CreateDevice(const char* name) {
  std::scoped_lock lock(m_mutex);

  // check for duplicates and don't overwrite them
  auto it = m_deviceMap.find(name);
  if (it != m_deviceMap.end()) return 0;

  // don't allow more than 4096 devices (limit driven by 12-bit allocation in
  // value changed callback uid)
  if (m_devices.size() >= 4095) return 0;

  // create and save
  auto deviceImpl = std::make_shared<Device>(name);
  HAL_SimDeviceHandle deviceHandle = m_devices.emplace_back(deviceImpl) + 1;
  deviceImpl->handle = deviceHandle;
  m_deviceMap[name] = deviceImpl;

  // notify callbacks
  m_deviceCreated(name, deviceHandle);

  return deviceHandle;
}

void SimDeviceData::FreeDevice(HAL_SimDeviceHandle handle) {
  std::scoped_lock lock(m_mutex);
  --handle;

  // see if it exists
  if (handle < 0 || static_cast<uint32_t>(handle) >= m_devices.size()) return;
  auto deviceImpl = std::move(m_devices[handle]);
  if (!deviceImpl) return;

  // remove from map
  m_deviceMap.erase(deviceImpl->name);

  // remove from vector
  m_devices.erase(handle);

  // notify callbacks
  m_deviceFreed(deviceImpl->name.c_str(), handle + 1);
}

HAL_SimValueHandle SimDeviceData::CreateValue(HAL_SimDeviceHandle device,
                                              const char* name, bool readonly,
                                              int32_t numOptions,
                                              const char** options,
                                              const HAL_Value& initialValue) {
  std::scoped_lock lock(m_mutex);

  // look up device
  Device* deviceImpl = LookupDevice(device);
  if (!deviceImpl) return 0;

  // check for duplicates and don't overwrite them
  auto it = deviceImpl->valueMap.find(name);
  if (it != deviceImpl->valueMap.end()) return 0;

  // don't allow more than 4096 values per device (limit driven by 12-bit
  // allocation in value changed callback uid)
  if (deviceImpl->values.size() >= 4095) return 0;

  // create and save; encode device into handle
  auto valueImplPtr = std::make_unique<Value>(name, readonly, initialValue);
  Value* valueImpl = valueImplPtr.get();
  HAL_SimValueHandle valueHandle =
      (device << 16) |
      (deviceImpl->values.emplace_back(std::move(valueImplPtr)) + 1);
  valueImpl->handle = valueHandle;
  // copy options (if any provided)
  if (numOptions > 0 && options) {
    valueImpl->enumOptions.reserve(numOptions);
    valueImpl->cstrEnumOptions.reserve(numOptions);
    for (int32_t i = 0; i < numOptions; ++i) {
      valueImpl->enumOptions.emplace_back(options[i]);
      // point to our copy of the string, not the passed-in one
      valueImpl->cstrEnumOptions.emplace_back(
          valueImpl->enumOptions.back().c_str());
    }
  }
  deviceImpl->valueMap[name] = valueImpl;

  // notify callbacks
  deviceImpl->valueCreated(name, valueHandle, readonly, &initialValue);

  return valueHandle;
}

HAL_Value SimDeviceData::GetValue(HAL_SimValueHandle handle) {
  std::scoped_lock lock(m_mutex);
  Value* valueImpl = LookupValue(handle);

  if (!valueImpl) {
    HAL_Value value;
    value.data.v_int = 0;
    value.type = HAL_UNASSIGNED;
    return value;
  }

  return valueImpl->value;
}

void SimDeviceData::SetValue(HAL_SimValueHandle handle,
                             const HAL_Value& value) {
  std::scoped_lock lock(m_mutex);
  Value* valueImpl = LookupValue(handle);
  if (!valueImpl) return;

  valueImpl->value = value;

  // notify callbacks
  valueImpl->changed(valueImpl->name.c_str(), valueImpl->handle,
                     valueImpl->readonly, &value);
}

int32_t SimDeviceData::RegisterDeviceCreatedCallback(
    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
    bool initialNotify) {
  std::scoped_lock lock(m_mutex);

  // register callback
  int32_t index = m_deviceCreated.Register(prefix, param, callback);

  // initial notifications
  if (initialNotify) {
    for (auto&& device : m_devices)
      callback(device->name.c_str(), param, device->handle);
  }

  return index;
}

void SimDeviceData::CancelDeviceCreatedCallback(int32_t uid) {
  if (uid <= 0) return;
  std::scoped_lock lock(m_mutex);
  m_deviceCreated.Cancel(uid);
}

int32_t SimDeviceData::RegisterDeviceFreedCallback(
    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
  std::scoped_lock lock(m_mutex);
  return m_deviceFreed.Register(prefix, param, callback);
}

void SimDeviceData::CancelDeviceFreedCallback(int32_t uid) {
  if (uid <= 0) return;
  std::scoped_lock lock(m_mutex);
  m_deviceFreed.Cancel(uid);
}

HAL_SimDeviceHandle SimDeviceData::GetDeviceHandle(const char* name) {
  std::scoped_lock lock(m_mutex);
  auto it = m_deviceMap.find(name);
  if (it == m_deviceMap.end()) return 0;
  if (auto deviceImpl = it->getValue().lock())
    return deviceImpl->handle;
  else
    return 0;
}

const char* SimDeviceData::GetDeviceName(HAL_SimDeviceHandle handle) {
  std::scoped_lock lock(m_mutex);

  // look up device
  Device* deviceImpl = LookupDevice(handle);
  if (!deviceImpl) return nullptr;

  return deviceImpl->name.c_str();
}

void SimDeviceData::EnumerateDevices(const char* prefix, void* param,
                                     HALSIM_SimDeviceCallback callback) {
  std::scoped_lock lock(m_mutex);
  for (auto&& device : m_devices) {
    if (wpi::StringRef{device->name}.startswith(prefix))
      callback(device->name.c_str(), param, device->handle);
  }
}

int32_t SimDeviceData::RegisterValueCreatedCallback(
    HAL_SimDeviceHandle device, void* param, HALSIM_SimValueCallback callback,
    bool initialNotify) {
  std::scoped_lock lock(m_mutex);
  Device* deviceImpl = LookupDevice(device);
  if (!deviceImpl) return -1;

  // register callback
  int32_t index = deviceImpl->valueCreated.Register(callback, param);

  // initial notifications
  if (initialNotify) {
    for (auto&& value : deviceImpl->values)
      callback(value->name.c_str(), param, value->handle, value->readonly,
               &value->value);
  }

  // encode device into uid
  return (device << 16) | (index & 0xffff);
}

void SimDeviceData::CancelValueCreatedCallback(int32_t uid) {
  if (uid <= 0) return;
  std::scoped_lock lock(m_mutex);
  Device* deviceImpl = LookupDevice(uid >> 16);
  if (!deviceImpl) return;
  deviceImpl->valueCreated.Cancel(uid & 0xffff);
}

int32_t SimDeviceData::RegisterValueChangedCallback(
    HAL_SimValueHandle handle, void* param, HALSIM_SimValueCallback callback,
    bool initialNotify) {
  std::scoped_lock lock(m_mutex);
  Value* valueImpl = LookupValue(handle);
  if (!valueImpl) return -1;

  // register callback
  int32_t index = valueImpl->changed.Register(callback, param);

  // initial notification
  if (initialNotify)
    callback(valueImpl->name.c_str(), param, valueImpl->handle,
             valueImpl->readonly, &valueImpl->value);

  // encode device and value into uid
  return (((handle >> 16) & 0xfff) << 19) | ((handle & 0xfff) << 7) |
         (index & 0x7f);
}

void SimDeviceData::CancelValueChangedCallback(int32_t uid) {
  if (uid <= 0) return;
  std::scoped_lock lock(m_mutex);
  Value* valueImpl = LookupValue(((uid >> 19) << 16) | ((uid >> 7) & 0xfff));
  if (!valueImpl) return;
  valueImpl->changed.Cancel(uid & 0x7f);
}

HAL_SimValueHandle SimDeviceData::GetValueHandle(HAL_SimDeviceHandle device,
                                                 const char* name) {
  std::scoped_lock lock(m_mutex);
  Device* deviceImpl = LookupDevice(device);
  if (!deviceImpl) return 0;

  // lookup value
  auto it = deviceImpl->valueMap.find(name);
  if (it == deviceImpl->valueMap.end()) return 0;
  if (!it->getValue()) return 0;
  return it->getValue()->handle;
}

void SimDeviceData::EnumerateValues(HAL_SimDeviceHandle device, void* param,
                                    HALSIM_SimValueCallback callback) {
  std::scoped_lock lock(m_mutex);
  Device* deviceImpl = LookupDevice(device);
  if (!deviceImpl) return;

  for (auto&& value : deviceImpl->values)
    callback(value->name.c_str(), param, value->handle, value->readonly,
             &value->value);
}

const char** SimDeviceData::GetValueEnumOptions(HAL_SimValueHandle handle,
                                                int32_t* numOptions) {
  *numOptions = 0;

  std::scoped_lock lock(m_mutex);
  Value* valueImpl = LookupValue(handle);
  if (!valueImpl) return nullptr;

  // get list of options (safe to return as they never change)
  auto& options = valueImpl->cstrEnumOptions;
  *numOptions = options.size();
  return options.data();
}

void SimDeviceData::ResetData() {
  std::scoped_lock lock(m_mutex);
  m_devices.clear();
  m_deviceMap.clear();
  m_deviceCreated.Reset();
  m_deviceFreed.Reset();
}

extern "C" {

int32_t HALSIM_RegisterSimDeviceCreatedCallback(
    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
    HAL_Bool initialNotify) {
  return SimSimDeviceData->RegisterDeviceCreatedCallback(
      prefix, param, callback, initialNotify);
}

void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {
  SimSimDeviceData->CancelDeviceCreatedCallback(uid);
}

int32_t HALSIM_RegisterSimDeviceFreedCallback(
    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
  return SimSimDeviceData->RegisterDeviceFreedCallback(prefix, param, callback);
}

void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {
  SimSimDeviceData->CancelDeviceFreedCallback(uid);
}

HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) {
  return SimSimDeviceData->GetDeviceHandle(name);
}

const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) {
  return SimSimDeviceData->GetDeviceName(handle);
}

HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
  if (handle <= 0) return 0;
  return handle >> 16;
}

void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
                                HALSIM_SimDeviceCallback callback) {
  SimSimDeviceData->EnumerateDevices(prefix, param, callback);
}

int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
                                               void* param,
                                               HALSIM_SimValueCallback callback,
                                               HAL_Bool initialNotify) {
  return SimSimDeviceData->RegisterValueCreatedCallback(device, param, callback,
                                                        initialNotify);
}

void HALSIM_CancelSimValueCreatedCallback(int32_t uid) {
  SimSimDeviceData->CancelValueCreatedCallback(uid);
}

int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
                                               void* param,
                                               HALSIM_SimValueCallback callback,
                                               HAL_Bool initialNotify) {
  return SimSimDeviceData->RegisterValueChangedCallback(handle, param, callback,
                                                        initialNotify);
}

void HALSIM_CancelSimValueChangedCallback(int32_t uid) {
  SimSimDeviceData->CancelValueChangedCallback(uid);
}

HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
                                            const char* name) {
  return SimSimDeviceData->GetValueHandle(device, name);
}

void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
                               HALSIM_SimValueCallback callback) {
  SimSimDeviceData->EnumerateValues(device, param, callback);
}

const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
                                           int32_t* numOptions) {
  return SimSimDeviceData->GetValueEnumOptions(handle, numOptions);
}

void HALSIM_ResetSimDeviceData(void) { SimSimDeviceData->ResetData(); }

}  // extern "C"