path: root/src/camera/ov9281.cpp

#include "ov9281.h"

#include <iostream>
#include <thread>

#include <string.h>
#include <sys/mman.h>

#include <libcamera/camera.h>
#include <libcamera/camera_manager.h>
#include <libcamera/control_ids.h>
#include <libcamera/framebuffer_allocator.h>
#include <libcamera/request.h>

#include "image.h"
#include "macro.h"
#include "pixels.h"
#include "rotaryencoder.h"
#include "typedefs.h"

#include <QElapsedTimer>

QElapsedTimer timer;
size_t fpsCounter{0};

OV9281::OV9281(const std::shared_ptr<libcamera::Camera> &camera)
    : INIT_FIELD(camera)
{
    timer.start();
    std::cout << __func__ << ":\tid: " << m_camera->id();
}

OV9281::~OV9281()
{
    for (auto &[fd, mem] : m_mappedBuffers)
    {
        munmap(mem.first, mem.second);
    }

    m_camera->release();
}

bool OV9281::init()
{
    if (m_camera->acquire() != EXIT_SUCCESS)
    {
        std::cerr << __func__ << ": " << m_camera->id()
                  << ": cannot acquire camera." << std::endl;

        return false;
    }

    m_config = m_camera->generateConfiguration({libcamera::StreamRole::Raw});

    if (m_config->empty())
    {
        std::cerr << __func__ << ": " << m_camera->id()
                  << ": cannot generate configuration" << std::endl;

        return false;
    }

    // m_config->orientation = libcamera::Orientation::Rotate90;

    libcamera::StreamConfiguration &streamConfig = m_config->at(0);

    std::cout << "supported pixel formats:\n";

    for (const auto &pixelFormat : streamConfig.formats().pixelformats())
    {
        std::cout << "\t" << pixelFormat.toString() << std::endl;

        for (const auto &size : streamConfig.formats().sizes(pixelFormat))
        {
            std::cout << "\t\t" << size.toString() << std::endl;
        }
    }

    streamConfig.pixelFormat = OV9281::pixelFormat;
    streamConfig.bufferCount = OV9281::bufferCount;

    if (!validateConfig())
    {
        return false;
    }

    if (!applyConfig()) {
        return false;
    }

    return true;
}

bool OV9281::validateConfig()
{
    using namespace libcamera;

    auto status = m_config->validate();

    // WARNING: unsafe

    switch (status)
    {
    case CameraConfiguration::Status::Valid: {
        std::cout << __func__ << ": " << "config is valid" << std::endl;
        break;
    }
    case CameraConfiguration::Status::Adjusted: {
        if (m_config->empty())
        {
            std::cerr << __func__ << ": " << "config is adjusted, but empty"
                      << std::endl;
            return false;
        }

        libcamera::StreamConfiguration &streamConfig = m_config->at(0);
        std::cout << __func__
                  << ":\tpixelFormat: " << streamConfig.pixelFormat.toString()
                  << std::endl;
        std::cout << __func__ << ":\tbufferCount: " << streamConfig.bufferCount
                  << std::endl;
        std::cout << __func__ << ":\torientation: " << m_config->orientation
                  << std::endl;
        break;
    }
    case CameraConfiguration::Status::Invalid: {
        std::cerr << __func__ << ":\tconfig is invalid" << std::endl;

        return false;
    }
    }

    return true;
}

bool OV9281::applyConfig()
{
    // FIXME: may crassh even on success (e.g. by setting pixelFormat to "8")
    if (m_camera->configure(m_config.get()) != EXIT_SUCCESS)
    {
        std::cerr << __func__ << ":\tcannot apply config" << std::endl;

        return false;
    }

    return true;
}

/*
 * Signals operate in the libcamera CameraManager thread context, so it is
 * important not to block the thread for a long time, as this blocks internal
 * processing of the camera pipelines, and can affect realtime performance.
 */
void OV9281::onRequestCompleted(libcamera::Request *completed_request)
{
    fpsCounter++;

    if (timer.elapsed() > 1000) {
        std::cout << "fps: " << fpsCounter << std::endl;
        timer.restart();
        fpsCounter = 0;
    }

    using namespace libcamera;

    if (completed_request->status() == Request::RequestCancelled)
    {
        std::cerr << __func__ << ":\trequest canceled" << std::endl;

        return;
    }

    const auto &buffers = completed_request->buffers();

#if 1
    for (auto [stream, buffer] : buffers)
    {
        const auto &streamConfig = stream->configuration();
        const auto &imageSize = streamConfig.size;
        const auto &pixelFormat = streamConfig.pixelFormat;
        const auto &stride = streamConfig.stride;

        const FrameMetadata &metadata = buffer->metadata();

        for (size_t i = 0; i < buffer->planes().size(); ++i)
        {
            const FrameBuffer::Plane &plane = buffer->planes()[i];
            const FrameMetadata::Plane &metaplane = buffer->metadata()
                                                        .planes()[i];

            size_t size = std::min(metaplane.bytesused, plane.length);
            // std::cout << "size is: " << size << std::endl;
            void *data = m_mappedBuffers[plane.fd.get()].first;

            auto img = std::make_shared<Image>();

            img->width = imageSize.width;
            img->height = imageSize.height;

            // img->dataSize = size;
            // img->stride = stride;
            img->pixelFormat = pixelFormat;
            img->counters.measurementCounter = metadata.sequence;
            img->counters.timestampUs = metadata.timestamp / 1000;
            img->counters.encoderPosition = RotaryEncoder::instance()->position();

            img->copyFromData(data, size);
            // memcpy(img->data, data, size);
            img->rotate();

            auto pixels = img->pixels();
// FIXME: backup emit value
#ifdef emit
#undef emit
            if (!pixels) {
                std::cerr << "emit empty pixels" << std::endl;
            }
            newImage.emit(img);
            // newPixels.emit(pixels);
#define emit
#endif
        }
    }
#endif
    const libcamera::ControlList &metadata = completed_request->metadata();
    const ControlInfoMap &control_map = m_camera->controls();
    // const ControlIdMap & ctrlIdMap = control_map.idmap();

    auto frameDurationCtrl = control_map.find(&controls::FrameDurationLimits);
    double fps = frameDurationCtrl == control_map.end() ?
                     std::numeric_limits<double>::quiet_NaN() :
                     (1e6 / frameDurationCtrl->second.min().get<int64_t>());

    auto exp = metadata.get(controls::ExposureTime);
    auto ag = metadata.get(controls::AnalogueGain);
    auto ae = metadata.get(controls::AeEnable);
    // auto br= metadata.get(controls::Brightness);
    static auto lastControls = completed_request->controls();

    completed_request->reuse(Request::ReuseBuffers);
    // completed_request->controls().set(libcamera::controls::AeEnable, false);
    // completed_request->controls()
    //     .set(libcamera::controls::draft ::NoiseReductionMode,
    //          libcamera::controls::draft ::NoiseReductionModeEnum ::
    //              NoiseReductionModeHighQuality);

    completed_request->controls().set(libcamera::controls::ExposureTime,
                                      m_exposureTime);

    m_camera->queueRequest(completed_request);
}

std::vector<std::shared_ptr<OV9281>> OV9281::search(
    std::unique_ptr<libcamera::CameraManager> &manager)
{
    std::vector<std::shared_ptr<OV9281>> result;

    for (const auto &camera : manager->cameras())
    {
        auto id = camera->id();
        auto c = manager->get(id);
        // auto ov9281 = std::shared_ptr<OV9281>(new OV9281(c));

        // if (!ov9281->init())
        // {
        //     continue;
        // }

        // result.push_back(ov9281);
    }

    return result;
}

bool OV9281::startStream()
{
    if (m_config->empty())
    {
        std::cerr << __func__ << ":\tconfig is empty" << std::endl;
        return false;
    }

    auto &streamConfig = m_config->at(0);
    m_allocator = std::make_unique<libcamera::FrameBufferAllocator>(m_camera);
    auto stream = streamConfig.stream();
    auto ret = m_allocator->allocate(stream);

    // TODO: check if zero
    if (ret < 0)
    {
        std::cerr << __func__ << ":\tcan't allocate buffers: " << strerror(ret)
                  << std::endl;
        return false;
    }

    auto allocatedCount = ret;
    std::cout << __func__ << ":\tallocated " << allocatedCount
              << " buffers for stream" << std::endl;

    const auto &buffers = m_allocator->buffers(stream);

    for (const auto &buffer : buffers)
    {
        auto request = m_camera->createRequest();

        if (!request)
        {
            std::cerr << __func__ << ":\tcan't create request" << std::endl;

            return false;
        }

        // TODO: try multiple buffers per request and compare performance
        ret = request->addBuffer(stream, buffer.get());

        if (ret < 0)
        {
            std::cerr << __func__
                      << ":\tcan't set buffer for request: " << strerror(ret)
                      << std::endl;

            return false;
        }

        for (const auto &plane : buffer->planes())
        {
            void *memory = mmap(NULL,
                                plane.length,
                                PROT_READ,
                                MAP_SHARED,
                                plane.fd.get(),
                                0);
            m_mappedBuffers[plane.fd.get()] = std::make_pair(memory,
                                                             plane.length);
        }

        std::int64_t lowerUS = 1 * 1000 * 1000 / desiredFPS;
        std::int64_t higherUS = lowerUS;
        std::int64_t value_pair[2] = {higherUS / 2, higherUS};
        request->controls().set(libcamera::controls::AnalogueGain, 1.0);
        request->controls().set(libcamera::controls::ExposureTime, 100);
        request->controls().set(libcamera::controls::FrameDurationLimits,
                                libcamera::Span<const std::int64_t, 2>(
                                    value_pair));

        m_requests.push_back(std::move(request));
    }

    m_camera->requestCompleted.connect(this, &OV9281::onRequestCompleted);

    // FIXME: memleak
    std::unique_ptr<libcamera::ControlList> camcontrols{
        new libcamera::ControlList()};

    {
        using namespace std::chrono_literals;
        std::this_thread::sleep_for(500ms);
    }

    ret = m_camera->start(camcontrols.get());

    if (ret)
    {
        std::cerr << __func__ << ":\tfailed to start camera: " << strerror(ret)
                  << std::endl;

        return false;
    }

    for (auto &request : m_requests)
    {
        ret = m_camera->queueRequest(request.get());

        if (ret)
        {
            std::cerr << __func__
                      << ":\tfailed to queue request: " << strerror(ret)
                      << std::endl;

            return false;
        }
    }

    return true;
}

void OV9281::printControls()
{
    using namespace libcamera;
    const libcamera::ControlInfoMap &control_map = m_camera->controls();

    // for (const auto & [id, info]: control_map)
    for (const std::pair<const ControlId *, ControlInfo> &pair : control_map)
    {
        const ControlId *const &id = pair.first;
        const ControlInfo &info = pair.second;

        std::cout << "\tc " << id->name() << " (" << id->id()
                  << "): " << info.toString()
                  << (info.def().isNone() ?
                          "" :
                          " (dflt:" + info.def().toString() + ")");

        if (!info.values().size())
        {
            std::cout << std::endl;
            continue;
        }

        std::cout << " - [";

        for (const auto &v : info.values())
        {
            std::cout << " " << v.toString();
        }

        std::cout << " ]\n";
    }
}