#include "ScanDataCache.h"

#include "LaserDataLoader.h"
#include "DebugDataSaver.h"
#include <cstring>
#include <memory>

namespace {
template <typename Point3D, typename Point2D>
bool CopyTyped(const SVzLaserLineData& source, SVzLaserLineData& destination)
{
    destination = source;
    destination.p3DPoint = nullptr;
    destination.p2DPoint = nullptr;
    if (source.nPointCount <= 0) return true;
    if (!source.p3DPoint) return false;

    auto points3D = std::make_unique<Point3D[]>(source.nPointCount);
    auto points2D = std::make_unique<Point2D[]>(source.nPointCount);
    std::memcpy(points3D.get(), source.p3DPoint,
                sizeof(Point3D) * static_cast<std::size_t>(source.nPointCount));
    if (source.p2DPoint) {
        std::memcpy(points2D.get(), source.p2DPoint,
                    sizeof(Point2D) * static_cast<std::size_t>(source.nPointCount));
    }
    destination.p3DPoint = points3D.release();
    destination.p2DPoint = points2D.release();
    return true;
}
}

ScanDataCache::ScanDataCache()
    : m_worker(&ScanDataCache::WorkerLoop, this)
{
}

ScanDataCache::~ScanDataCache()
{
    Stop();
    Clear();
}

void ScanDataCache::BeginScan(ScanId scanId)
{
    (void)scanId;
    Drain();
    {
        std::lock_guard<std::mutex> queueLock(m_queueMutex);
        while (!m_queue.empty()) {
            Data owned;
            owned.emplace_back(m_queue.front().dataType, m_queue.front().line);
            FreeData(owned);
            m_queue.pop_front();
        }
        m_queuedBytes = 0;
    }
    std::lock_guard<std::mutex> lock(m_stateMutex);
    for (auto& entry : m_buffers) {
        FreeData(entry.second.data);
    }
    m_buffers.clear();
}

bool ScanDataCache::TryEnqueue(ScanId scanId, int cameraIndex,
                               EVzResultDataType dataType,
                               const SVzLaserLineData& source)
{
    QueueItem item;
    item.scanId = scanId;
    item.cameraIndex = cameraIndex;
    item.dataType = dataType;
    try {
        if (!CopyLine(dataType, source, item.line)) return false;
    } catch (...) {
        return false;
    }
    item.bytes = static_cast<std::size_t>(DebugDataSaver::GetLineStorageSize(dataType, item.line));

    std::unique_lock<std::mutex> lock(m_queueMutex);
    if (!m_running.load() || item.bytes == 0) {
        FreeLine(dataType, item.line);
        return false;
    }

    const std::size_t queuedBytes = item.bytes;
    try {
        m_queue.emplace_back(std::move(item));
    } catch (...) {
        FreeLine(dataType, item.line);
        return false;
    }
    {
        std::lock_guard<std::mutex> stateLock(m_stateMutex);
        try {
            auto& buffer = m_buffers[{scanId, cameraIndex}];
            ++buffer.pending;
        } catch (...) {
            QueueItem rejected = std::move(m_queue.back());
            m_queue.pop_back();
            FreeLine(rejected.dataType, rejected.line);
            return false;
        }
    }
    m_queuedBytes += queuedBytes;
    lock.unlock();
    m_queueCondition.notify_one();
    return true;
}

void ScanDataCache::FinishCamera(ScanId scanId, int cameraIndex)
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    auto& buffer = m_buffers[{scanId, cameraIndex}];
    buffer.finishRequested = true;
    buffer.completed = buffer.pending == 0;
}

bool ScanDataCache::TryTakeCompleted(ScanId scanId, int cameraIndex, Data& data)
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    auto it = m_buffers.find({scanId, cameraIndex});
    if (it == m_buffers.end() || !it->second.completed || it->second.data.empty()) return false;
    data = std::move(it->second.data);
    it->second.data.clear();
    it->second.completed = false;
    return true;
}

void ScanDataCache::Restore(ScanId scanId, int cameraIndex, Data&& data)
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    auto& buffer = m_buffers[{scanId, cameraIndex}];
    FreeData(buffer.data);
    buffer.data = std::move(data);
    buffer.completed = true;
}

void ScanDataCache::ReplaceCompleted(ScanId scanId, int cameraIndex, Data&& data)
{
    Restore(scanId, cameraIndex, std::move(data));
}

void ScanDataCache::SetLineObserver(
    std::function<void(ScanId, int, EVzResultDataType, const SVzLaserLineData&)> observer)
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    m_lineObserver = std::move(observer);
}

void ScanDataCache::SetReadyNotifier(std::function<void(ScanId, int)> notifier)
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    m_readyNotifier = std::move(notifier);
}

bool ScanDataCache::CopyData(ScanId scanId, int cameraIndex, Data& data) const
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    const auto it = m_buffers.find({scanId, cameraIndex});
    if (it == m_buffers.end()) return false;
    for (const auto& line : it->second.data) {
        SVzLaserLineData copy{};
        try {
            if (!CopyLine(line.first, line.second, copy)) {
                FreeData(data);
                return false;
            }
            data.emplace_back(line.first, copy);
        } catch (...) {
            Data owned;
            owned.emplace_back(line.first, copy);
            FreeData(owned);
            FreeData(data);
            return false;
        }
    }
    return !data.empty();
}

bool ScanDataCache::HasData(ScanId scanId, int cameraIndex) const
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    const auto it = m_buffers.find({scanId, cameraIndex});
    return it != m_buffers.end() && !it->second.data.empty();
}

bool ScanDataCache::IsCompleted(ScanId scanId, int cameraIndex) const
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    const auto it = m_buffers.find({scanId, cameraIndex});
    return it != m_buffers.end() && it->second.completed;
}

bool ScanDataCache::IsIncomplete(ScanId scanId, int cameraIndex) const
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    const auto it = m_buffers.find({scanId, cameraIndex});
    return it != m_buffers.end() && it->second.incomplete;
}

std::size_t ScanDataCache::Size(ScanId scanId, int cameraIndex) const
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    const auto it = m_buffers.find({scanId, cameraIndex});
    return it == m_buffers.end() ? 0 : it->second.data.size();
}

void ScanDataCache::CancelScan(ScanId scanId)
{
    Drain();
    {
        std::lock_guard<std::mutex> queueLock(m_queueMutex);
        std::deque<QueueItem> retained;
        while (!m_queue.empty()) {
            QueueItem item = std::move(m_queue.front());
            m_queue.pop_front();
            if (item.scanId == scanId) {
                Data owned;
                owned.emplace_back(item.dataType, item.line);
                FreeData(owned);
            } else {
                retained.push_back(std::move(item));
            }
        }
        m_queue.swap(retained);
        m_queuedBytes = 0;
        std::deque<QueueItem> measure = m_queue;
        while (!measure.empty()) {
            m_queuedBytes += measure.front().bytes;
            measure.pop_front();
        }
    }
    std::lock_guard<std::mutex> lock(m_stateMutex);
    for (auto it = m_buffers.begin(); it != m_buffers.end();) {
        if (it->first.first == scanId) {
            FreeData(it->second.data);
            it = m_buffers.erase(it);
        } else ++it;
    }
}

void ScanDataCache::Clear()
{
    {
        std::lock_guard<std::mutex> lock(m_queueMutex);
        while (!m_queue.empty()) {
            Data owned;
            owned.emplace_back(m_queue.front().dataType, m_queue.front().line);
            FreeData(owned);
            m_queue.pop_front();
        }
        m_queuedBytes = 0;
    }
    std::lock_guard<std::mutex> lock(m_stateMutex);
    for (auto& entry : m_buffers) FreeData(entry.second.data);
    m_buffers.clear();
}

void ScanDataCache::Drain()
{
    std::unique_lock<std::mutex> lock(m_queueMutex);
    m_queueCondition.wait(lock, [this]() {
        return m_queue.empty() && m_inFlight == 0;
    });
}

void ScanDataCache::Stop()
{
    if (!m_running.exchange(false)) return;
    m_queueCondition.notify_all();
    if (m_worker.joinable()) m_worker.join();
}

void ScanDataCache::WorkerLoop()
{
    while (true) {
        QueueItem item;
        {
            std::unique_lock<std::mutex> lock(m_queueMutex);
            m_queueCondition.wait(lock, [this]() {
                return !m_running.load() || !m_queue.empty();
            });
            if (!m_running.load() && m_queue.empty()) break;
            item = std::move(m_queue.front());
            m_queue.pop_front();
            m_queuedBytes -= item.bytes;
            ++m_inFlight;
        }

        std::function<void(ScanId, int, EVzResultDataType, const SVzLaserLineData&)> observer;
        {
            std::lock_guard<std::mutex> lock(m_stateMutex);
            observer = m_lineObserver;
        }
        if (observer) {
            try { observer(item.scanId, item.cameraIndex, item.dataType, item.line); }
            catch (...) { }
        }

        std::function<void(ScanId, int)> notifier;
        {
            std::lock_guard<std::mutex> lock(m_stateMutex);
            auto& buffer = m_buffers[{item.scanId, item.cameraIndex}];
            try {
                buffer.data.emplace_back(item.dataType, item.line);
                item.line = SVzLaserLineData{};
            } catch (...) {
                buffer.incomplete = true;
            }
            if (buffer.pending > 0) --buffer.pending;
            if (buffer.finishRequested && buffer.pending == 0) buffer.completed = true;
            if (buffer.completed) notifier = m_readyNotifier;
        }
        if (item.line.p3DPoint || item.line.p2DPoint) {
            FreeLine(item.dataType, item.line);
        }
        if (notifier) notifier(item.scanId, item.cameraIndex);
        {
            std::lock_guard<std::mutex> lock(m_queueMutex);
            --m_inFlight;
        }
        m_queueCondition.notify_all();
    }
}

bool ScanDataCache::CopyLine(EVzResultDataType type,
                             const SVzLaserLineData& source,
                             SVzLaserLineData& destination)
{
    switch (type) {
    case keResultDataType_Position:
        return CopyTyped<SVzNL3DPosition, SVzNL2DPosition>(source, destination);
    case keResultDataType_PositionF:
        return CopyTyped<SVzNL3DPosition, SVzNL2DPositionF>(source, destination);
    case keResultDataType_PointXYZRGBA:
        return CopyTyped<SVzNLPointXYZRGBA, SVzNL2DLRPoint>(source, destination);
    case keResultDataType_PointXYZI:
        return CopyTyped<SVzNLPointXYZI, SVzNL2DLRPoint>(source, destination);
    default:
        return false;
    }
}

void ScanDataCache::FreeData(Data& data)
{
    LaserDataLoader loader;
    loader.FreeLaserScanData(data);
    data.clear();
}

void ScanDataCache::FreeLine(EVzResultDataType type, SVzLaserLineData& line)
{
    switch (type) {
    case keResultDataType_Position:
        delete[] static_cast<SVzNL3DPosition*>(line.p3DPoint);
        delete[] static_cast<SVzNL2DPosition*>(line.p2DPoint);
        break;
    case keResultDataType_PositionF:
        delete[] static_cast<SVzNL3DPosition*>(line.p3DPoint);
        delete[] static_cast<SVzNL2DPositionF*>(line.p2DPoint);
        break;
    case keResultDataType_PointXYZRGBA:
        delete[] static_cast<SVzNLPointXYZRGBA*>(line.p3DPoint);
        delete[] static_cast<SVzNL2DLRPoint*>(line.p2DPoint);
        break;
    case keResultDataType_PointXYZI:
        delete[] static_cast<SVzNLPointXYZI*>(line.p3DPoint);
        delete[] static_cast<SVzNL2DLRPoint*>(line.p2DPoint);
        break;
    default:
        break;
    }
    line.p3DPoint = nullptr;
    line.p2DPoint = nullptr;
}