// 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.

#include "hal/DIO.h"

#include <cmath>
#include <cstdio>
#include <thread>

#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/cpp/fpga_clock.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"

using namespace hal;

// Create a mutex to protect changes to the DO PWM config
static wpi::mutex digitalPwmMutex;

static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
                             kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>*
    digitalPWMHandles;

namespace hal::init {
void InitializeDIO() {
  static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
                               kNumDigitalPWMOutputs,
                               HAL_HandleEnum::DigitalPWM>
      dpH;
  digitalPWMHandles = &dpH;
}
}  // namespace hal::init

extern "C" {

HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
                                        HAL_Bool input,
                                        const char* allocationLocation,
                                        int32_t* status) {
  hal::init::CheckInit();
  initializeDigital(status);

  if (*status != 0) {
    return HAL_kInvalidHandle;
  }

  int16_t channel = getPortHandleChannel(portHandle);
  if (channel == InvalidHandleIndex || channel >= kNumDigitalChannels) {
    *status = RESOURCE_OUT_OF_RANGE;
    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
                                     kNumDigitalChannels, channel);
    return HAL_kInvalidHandle;
  }

  HAL_DigitalHandle handle;

  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO,
                                              &handle, status);

  if (*status != 0) {
    if (port) {
      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
                                           port->previousAllocation);
    } else {
      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
                                       kNumDigitalChannels, channel);
    }
    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
  }

  port->channel = static_cast<uint8_t>(channel);

  std::scoped_lock lock(digitalDIOMutex);

  tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status);

  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    if (!getPortHandleSPIEnable(portHandle)) {
      // if this flag is not set, we actually want DIO.
      uint32_t bitToSet = 1u << remapSPIChannel(port->channel);

      uint16_t specialFunctions = spiSystem->readEnableDIO(status);
      // Set the field to enable SPI DIO
      spiSystem->writeEnableDIO(specialFunctions | bitToSet, status);

      if (input) {
        outputEnable.SPIPort =
            outputEnable.SPIPort & (~bitToSet);  // clear the field for read
      } else {
        outputEnable.SPIPort =
            outputEnable.SPIPort | bitToSet;  // set the bits for write
      }
    }
  } else if (port->channel < kNumDigitalHeaders) {
    uint32_t bitToSet = 1u << port->channel;
    if (input) {
      outputEnable.Headers =
          outputEnable.Headers & (~bitToSet);  // clear the bit for read
    } else {
      outputEnable.Headers =
          outputEnable.Headers | bitToSet;  // set the bit for write
    }
  } else {
    uint32_t bitToSet = 1u << remapMXPChannel(port->channel);

    uint16_t specialFunctions =
        digitalSystem->readEnableMXPSpecialFunction(status);
    digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet,
                                                 status);

    if (input) {
      outputEnable.MXP =
          outputEnable.MXP & (~bitToSet);  // clear the bit for read
    } else {
      outputEnable.MXP = outputEnable.MXP | bitToSet;  // set the bit for write
    }
  }

  digitalSystem->writeOutputEnable(outputEnable, status);
  port->previousAllocation = allocationLocation ? allocationLocation : "";

  return handle;
}

HAL_Bool HAL_CheckDIOChannel(int32_t channel) {
  return channel < kNumDigitalChannels && channel >= 0;
}

void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  // no status, so no need to check for a proper free.
  if (port == nullptr)
    return;
  digitalChannelHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);

  // Wait for no other object to hold this handle.
  auto start = hal::fpga_clock::now();
  while (port.use_count() != 1) {
    auto current = hal::fpga_clock::now();
    if (start + std::chrono::seconds(1) < current) {
      std::puts("DIO handle free timeout");
      std::fflush(stdout);
      break;
    }
    std::this_thread::yield();
  }

  int32_t status = 0;
  std::scoped_lock lock(digitalDIOMutex);
  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    // Unset the SPI flag
    int32_t bitToUnset = 1 << remapSPIChannel(port->channel);
    uint16_t specialFunctions = spiSystem->readEnableDIO(&status);
    spiSystem->writeEnableDIO(specialFunctions & ~bitToUnset, &status);
  } else if (port->channel >= kNumDigitalHeaders) {
    // Unset the MXP flag
    uint32_t bitToUnset = 1u << remapMXPChannel(port->channel);

    uint16_t specialFunctions =
        digitalSystem->readEnableMXPSpecialFunction(&status);
    digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToUnset,
                                                 &status);
  }
}

void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
}

HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status) {
  auto handle = digitalPWMHandles->Allocate();
  if (handle == HAL_kInvalidHandle) {
    *status = NO_AVAILABLE_RESOURCES;
    return HAL_kInvalidHandle;
  }

  auto id = digitalPWMHandles->Get(handle);
  if (id == nullptr) {  // would only occur on thread issue.
    *status = HAL_HANDLE_ERROR;
    return HAL_kInvalidHandle;
  }
  *id = static_cast<uint8_t>(getHandleIndex(handle));

  return handle;
}

void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status) {
  digitalPWMHandles->Free(pwmGenerator);
}

void HAL_SetDigitalPWMRate(double rate, int32_t* status) {
  // Currently rounding in the log rate domain... heavy weight toward picking a
  // higher freq.
  // TODO: Round in the linear rate domain.
  initializeDigital(status);
  if (*status != 0) {
    return;
  }
  uint16_t pwmPeriodPower =
      std::lround(std::log(1.0 / (16 * 1.0E-6 * rate)) / std::log(2.0));
  digitalSystem->writePWMPeriodPower(pwmPeriodPower, status);
}

void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
                                double dutyCycle, int32_t* status) {
  auto port = digitalPWMHandles->Get(pwmGenerator);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }
  int32_t id = *port;
  if (dutyCycle > 1.0) {
    dutyCycle = 1.0;
  }
  if (dutyCycle < 0.0) {
    dutyCycle = 0.0;
  }
  double rawDutyCycle = 256.0 * dutyCycle;
  if (rawDutyCycle > 255.5) {
    rawDutyCycle = 255.5;
  }
  {
    std::scoped_lock lock(digitalPwmMutex);
    uint16_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status);
    if (pwmPeriodPower < 4) {
      // The resolution of the duty cycle drops close to the highest
      // frequencies.
      rawDutyCycle = rawDutyCycle / std::pow(2.0, 4 - pwmPeriodPower);
    }
    if (id < 4)
      digitalSystem->writePWMDutyCycleA(id, static_cast<uint8_t>(rawDutyCycle),
                                        status);
    else
      digitalSystem->writePWMDutyCycleB(
          id - 4, static_cast<uint8_t>(rawDutyCycle), status);
  }
}

void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
                                    int32_t channel, int32_t* status) {
  auto port = digitalPWMHandles->Get(pwmGenerator);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }
  int32_t id = *port;
  if (channel >= kNumDigitalHeaders &&
      channel <
          kNumDigitalHeaders + kNumDigitalMXPChannels) {  // If it is on the MXP
    /* Then to write as a digital PWM channel an offset is needed to write on
     * the correct channel
     */
    channel += kMXPDigitalPWMOffset;
  }
  digitalSystem->writePWMOutputSelect(id, channel, status);
}

void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
                int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }
  if (value != 0 && value != 1) {
    if (value != 0) {
      value = 1;
    }
  }
  {
    std::scoped_lock lock(digitalDIOMutex);

    tDIO::tOutputEnable currentOutputEnable =
        digitalSystem->readOutputEnable(status);

    HAL_Bool isInput = false;

    if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
      isInput =
          ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
           1) == 0;
    } else if (port->channel < kNumDigitalHeaders) {
      isInput = ((currentOutputEnable.Headers >> port->channel) & 1) == 0;
    } else {
      isInput = ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) &
                 1) == 0;
    }

    if (isInput) {
      *status = PARAMETER_OUT_OF_RANGE;
      hal::SetLastError(status, "Cannot set output of an input channel");
      return;
    }

    tDIO::tDO currentDIO = digitalSystem->readDO(status);

    if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
      if (value == 0) {
        currentDIO.SPIPort =
            currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
      } else if (value == 1) {
        currentDIO.SPIPort =
            currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
      }
    } else if (port->channel < kNumDigitalHeaders) {
      if (value == 0) {
        currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
      } else if (value == 1) {
        currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
      }
    } else {
      if (value == 0) {
        currentDIO.MXP =
            currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
      } else if (value == 1) {
        currentDIO.MXP =
            currentDIO.MXP | (1u << remapMXPChannel(port->channel));
      }
    }
    digitalSystem->writeDO(currentDIO, status);
  }
}

void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
                         int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }
  {
    std::scoped_lock lock(digitalDIOMutex);
    tDIO::tOutputEnable currentDIO = digitalSystem->readOutputEnable(status);

    if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
      if (input) {
        currentDIO.SPIPort =
            currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
      } else {
        currentDIO.SPIPort =
            currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
      }
    } else if (port->channel < kNumDigitalHeaders) {
      if (input) {
        currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
      } else {
        currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
      }
    } else {
      if (input) {
        currentDIO.MXP =
            currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
      } else {
        currentDIO.MXP =
            currentDIO.MXP | (1u << remapMXPChannel(port->channel));
      }
    }
    digitalSystem->writeOutputEnable(currentDIO, status);
  }
}

HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return false;
  }
  tDIO::tDI currentDIO = digitalSystem->readDI(status);
  // Shift 00000001 over channel-1 places.
  // AND it against the currentDIO
  // if it == 0, then return false
  // else return true

  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    return ((currentDIO.SPIPort >> remapSPIChannel(port->channel)) & 1) != 0;
  } else if (port->channel < kNumDigitalHeaders) {
    return ((currentDIO.Headers >> port->channel) & 1) != 0;
  } else {
    return ((currentDIO.MXP >> remapMXPChannel(port->channel)) & 1) != 0;
  }
}

HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return false;
  }
  tDIO::tOutputEnable currentOutputEnable =
      digitalSystem->readOutputEnable(status);
  // Shift 00000001 over port->channel-1 places.
  // AND it against the currentOutputEnable
  // if it == 0, then return false
  // else return true

  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    return ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
            1) == 0;
  } else if (port->channel < kNumDigitalHeaders) {
    return ((currentOutputEnable.Headers >> port->channel) & 1) == 0;
  } else {
    return ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) & 1) ==
           0;
  }
}

void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
               int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }
  tDIO::tPulse pulse;

  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    pulse.SPIPort = 1u << remapSPIChannel(port->channel);
  } else if (port->channel < kNumDigitalHeaders) {
    pulse.Headers = 1u << port->channel;
  } else {
    pulse.MXP = 1u << remapMXPChannel(port->channel);
  }

  digitalSystem->writePulseLength(
      static_cast<uint16_t>(1.0e9 * pulseLength /
                            (pwmSystem->readLoopTiming(status) * 25)),
      status);
  digitalSystem->writePulse(pulse, status);
}

HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return false;
  }
  tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);

  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    return (pulseRegister.SPIPort & (1 << remapSPIChannel(port->channel))) != 0;
  } else if (port->channel < kNumDigitalHeaders) {
    return (pulseRegister.Headers & (1 << port->channel)) != 0;
  } else {
    return (pulseRegister.MXP & (1 << remapMXPChannel(port->channel))) != 0;
  }
}

HAL_Bool HAL_IsAnyPulsing(int32_t* status) {
  initializeDigital(status);
  if (*status != 0) {
    return false;
  }
  tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);
  return pulseRegister.Headers != 0 && pulseRegister.MXP != 0 &&
         pulseRegister.SPIPort != 0;
}

void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex,
                         int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  std::scoped_lock lock(digitalDIOMutex);
  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    // Channels 10-15 are SPI channels, so subtract our MXP channels
    digitalSystem->writeFilterSelectHdr(port->channel - kNumDigitalMXPChannels,
                                        filterIndex, status);
  } else if (port->channel < kNumDigitalHeaders) {
    digitalSystem->writeFilterSelectHdr(port->channel, filterIndex, status);
  } else {
    digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->channel),
                                        filterIndex, status);
  }
}

int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) {
  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  std::scoped_lock lock(digitalDIOMutex);
  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    // Channels 10-15 are SPI channels, so subtract our MXP channels
    return digitalSystem->readFilterSelectHdr(
        port->channel - kNumDigitalMXPChannels, status);
  } else if (port->channel < kNumDigitalHeaders) {
    return digitalSystem->readFilterSelectHdr(port->channel, status);
  } else {
    return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->channel),
                                              status);
  }
}

void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) {
  initializeDigital(status);
  if (*status != 0) {
    return;
  }
  std::scoped_lock lock(digitalDIOMutex);
  digitalSystem->writeFilterPeriodHdr(filterIndex, value, status);
  if (*status == 0) {
    digitalSystem->writeFilterPeriodMXP(filterIndex, value, status);
  }
}

int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status) {
  initializeDigital(status);
  if (*status != 0) {
    return 0;
  }
  uint32_t hdrPeriod = 0;
  uint32_t mxpPeriod = 0;
  {
    std::scoped_lock lock(digitalDIOMutex);
    hdrPeriod = digitalSystem->readFilterPeriodHdr(filterIndex, status);
    if (*status == 0) {
      mxpPeriod = digitalSystem->readFilterPeriodMXP(filterIndex, status);
    }
  }
  if (hdrPeriod != mxpPeriod) {
    *status = NiFpga_Status_SoftwareFault;
    return -1;
  }
  return hdrPeriod;
}

}  // extern "C"