#include "dbc_decoder.h"

const DecodeFrame *DbcDecoder::FindFrame (const DecodeDatabase &database,
    std::uint32_t canId,
    bool isExtended) const {
  const FrameKey key (canId, isExtended);
  const std::unordered_map<FrameKey, std::size_t, FrameKeyHasher>::const_iterator it =
    database.frameIndexByKey.find (key);

  if (it == database.frameIndexByKey.end())
    return nullptr;

  const std::size_t index = it->second;
  if (index >= database.frames.size())
    return nullptr;

  return &database.frames[index];
}

DecodedFrameValue DbcDecoder::Decode (const DecodeDatabase &database,
                                      const RawCanFrame &frame) const {
  DecodedFrameValue result;
  const DecodeFrame *definition = FindFrame (database, frame.canId, frame.isExtended);

  if (definition == nullptr)
    return result;

  result.definition = definition;
  result.valid = true;

  for (std::size_t signalIndex = 0U; signalIndex < definition->signals.size(); ++signalIndex) {
    const DecodeSignal &signal = definition->signals[signalIndex];
    DecodedSignalValue decoded;
    std::uint64_t unsignedValue = 0U;

    decoded.definition = &signal;

    if (!ExtractUnsigned (frame.data, signal, unsignedValue)) {
      decoded.valid = false;
      result.signals.push_back (decoded);
      continue;
    }

    if (signal.valueType == ValueType::Signed)
      decoded.rawValue = SignExtend (unsignedValue, signal.length);
    else
      decoded.rawValue = static_cast<std::int64_t> (unsignedValue);

    decoded.physicalValue =
      (static_cast<double> (decoded.rawValue) * signal.factor) + signal.offset;
    decoded.valid = true;

    result.signals.push_back (decoded);
  }

  return result;
}

bool DbcDecoder::ExtractUnsigned (const std::vector<std::uint8_t> &data,
                                  const DecodeSignal &signal,
                                  std::uint64_t &value) {
  if ((signal.length == 0U) || (signal.length > 64U))
    return false;

  if (signal.byteOrder == ByteOrder::Intel)
    return ExtractIntel (data, signal.startBit, signal.length, value);

  return ExtractMotorola (data, signal.startBit, signal.length, value);
}

bool DbcDecoder::ExtractIntel (const std::vector<std::uint8_t> &data,
                               std::uint32_t startBit,
                               std::uint32_t length,
                               std::uint64_t &value) {
  value = 0U;

  for (std::uint32_t bitIndex = 0U; bitIndex < length; ++bitIndex) {
    const std::uint32_t absoluteBit = startBit + bitIndex;
    const std::uint32_t byteIndex = absoluteBit / 8U;
    const std::uint32_t bitInByte = absoluteBit % 8U;

    if (byteIndex >= data.size())
      return false;

    const std::uint64_t bitValue =
      (static_cast<std::uint64_t> ((data[byteIndex] >> bitInByte) & 0x01U) << bitIndex);

    value |= bitValue;
  }

  return true;
}

bool DbcDecoder::ExtractMotorola (const std::vector<std::uint8_t> &data,
                                  std::uint32_t startBit,
                                  std::uint32_t length,
                                  std::uint64_t &value) {
  /*
   * DBC Motorola bit numbering:
   * - startBit points to the most significant bit of the signal
   * - inside a byte, bit numbering goes 7..0
   * - crossing byte boundary moves to the next byte, bit 7
   */

  value = 0U;

  std::int32_t currentBit = static_cast<std::int32_t> (startBit);

  for (std::uint32_t bitIndex = 0U; bitIndex < length; ++bitIndex) {
    if (currentBit < 0)
      return false;

    const std::uint32_t absoluteBit = static_cast<std::uint32_t> (currentBit);
    const std::uint32_t byteIndex = absoluteBit / 8U;
    const std::uint32_t bitFromMsb = absoluteBit % 8U;
    const std::uint32_t bitInByte = 7U - bitFromMsb;

    if (byteIndex >= data.size())
      return false;

    value <<= 1U;
    value |= static_cast<std::uint64_t> ((data[byteIndex] >> bitInByte) & 0x01U);

    if ((absoluteBit % 8U) == 7U)
      currentBit = static_cast<std::int32_t> ((byteIndex + 1U) * 8U);
    else
      --currentBit;
  }

  return true;
}

std::int64_t DbcDecoder::SignExtend (std::uint64_t value, std::uint32_t bitLength) {
  if ((bitLength == 0U) || (bitLength >= 64U))
    return static_cast<std::int64_t> (value);

  const std::uint64_t signMask = (static_cast<std::uint64_t> (1U) << (bitLength - 1U));
  const std::uint64_t valueMask = (static_cast<std::uint64_t> (1U) << bitLength) - 1U;

  value &= valueMask;

  if ((value & signMask) == 0U)
    return static_cast<std::int64_t> (value);

  return static_cast<std::int64_t> (value | (~valueMask));
}