path: root/main.cpp

#include <chrono>
#include <errno.h>
#include <fstream>
#include <iostream>
#include <iterator>
#include <string.h>
#include <sys/mman.h>
#include <thread>

#include "LibCamera.h"
#include "calibration.h"
#include "camera/innomakerov9281.h"
#include "camera/ov9281.h"
#include "dumps.h"
#include "fuck_intel.h"
#include "genetic_algos.h"
#include "httpservice.h"
#include "imagealgos.h"
#include "laser.h"
#include "macro.h"
#include "pigpio.h"
#include "printerclient.h"
#include "profile.h"
#include "rotaryencoder.h"

#include <QCoreApplication>
#include <QDebug>
#include <QDir>
#include <QFile>
#include <QHttpServer>
#include <QImage>
#include <QJsonArray>
#include <QJsonDocument>
#include <QJsonObject>
#include <QSerialPort>
#include <QTextStream>
#include <QTimer>
#include <QtConcurrent/QtConcurrent>

#define try_apply_config() \
    if (!applyConfig(config)) \
    { \
        camera->release(); \
        cm->stop(); \
\
        return EXIT_FAILURE; \
    }

ScanningModeFlags scanningModeFlags{ScanningModeFlags::None};

QElapsedTimer calibrationTimer;

extern volatile int32_t positionSteps;

requested_params_t requested_params;

namespace {
std::shared_ptr<Image> img;
Pixels pixels;
std::vector<Pixels> calibrationPixels;
QMutex calibrationPixelsMutex;
} // namespace

using namespace std::chrono_literals;

// static std::shared_ptr<libcamera::Camera> camera;
// std::unique_ptr<libcamera::CameraConfiguration> config;
// static std::map<int, std::pair<void*, unsigned int>> mappedBuffers_;
// std::vector<std::unique_ptr<libcamera::Request>> requests;
libcamera::ControlList lastControls;

namespace {
CalibrationTablePtr calibrationTableZ;
CalibrationTablePtr calibrationTableX;
} // namespace

// static bool applyConfig(
//     const std::unique_ptr<libcamera::CameraConfiguration>& config
// );
// static void onRequestCompleted(libcamera::Request* completed_request);
// static void printControls();
// static QList<Pixels> filter(const QList<Pixels>& rawProfiles);

auto printPixels = [](const auto& pixels) {
    for (size_t i = (img_width - 10) / 2;
         i < img_width - ((img_width - 10) / 2);
         ++i)
    {
        std::cout << pixels[i] << " ";
    }
    std::cout << std::endl;
};

void onNewImage(std::shared_ptr<Image> image)
{
    // std::cout << __func__ << std::endl << std::flush;

    if (!image)
    {
        qDebug() << __func__ << "no image";
        return;
    }

    ::img = image;
}

void onNewPixels(std::shared_ptr<Pixels> pixels)
{
    // std::cout << __func__ << std::endl << std::flush;

    if (!pixels)
    {
        qDebug() << __func__ << "got null pixels";
    }

    if (!*pixels)
    {
        // qDebug() << __func__ << "got empty pixels";
    }

    for (size_t i = 640 - 5; i < 640 + 5; i++)
    {
        // std::cout << pixels->pixels[i] << " ";
    }

    // std::cout << std::endl

    ::pixels = *pixels;
}

bool initLaser();

int main(int argc, char* argv[])
{
    QCoreApplication app(argc, argv);

    QList<QFuture<void>> initializers;

#ifdef INNO_MAKER
    if (false)
    {
        std::cout << std::boolalpha;
        InnoMakerOV9281 innoMakerCam;
        qDebug() << "init:" << innoMakerCam.init();
        qDebug() << "set exposure:" << innoMakerCam.setExposureTimeUs(3000);
        qDebug() << "set gain:" << innoMakerCam.setGain(3000);

        innoMakerCam.startStream();
        QThread::sleep(3);
        qDebug() << "should be stopped";
        // Image buf;

        // for (size_t i = 0; i < 1000; ++i) {
        //     if (!innoMakerCam.getImage(buf)) {
        //         break;
        //     }

        //     buf.rotate();
        //     auto pixels = buf.pixels();
        // }
    }
    // qDebug() << "ok";
    // exit(EXIT_SUCCESS);
#endif

    // if (false)
    qDebug() << "size of raw profile" << sizeof(Pixels);
    if (true)
    {
        // open binary calibration table
        if (true)
        {
            initializers << QtConcurrent::run([&]() {
                if (!openCalibrationTable(
                        "/home/user/dumps/binz.calibration_table",
                        ::calibrationTableZ))
                {
                    exit(EXIT_FAILURE);
                }
            });

            initializers << QtConcurrent::run([&]() {
                if (!openCalibrationTable(
                        "/home/user/dumps/binx.calibration_table",
                        ::calibrationTableX))
                {
                    exit(EXIT_FAILURE);
                }
            });
        }

        if (false)
        {
            // z
            // if (!openCalibrationTable(
            //         "/home/user/dumps/binz.calibration_table",
            //         ::calibrationTableZ
            //     ))
            // {
            //     exit(EXIT_FAILURE);
            // }

            // if (!calibrationTableToImage(::calibrationTableZ)
            //          .save("/home/user/dumps/imageZ.png"))
            // {
            //     qDebug() << "cannot save imageZ.png";
            //     exit(EXIT_FAILURE);
            // }

            // interpolate(::calibrationTableZ);
            // exit(EXIT_SUCCESS);

            // calibrationTableToImage(::calibrationTableZ)
            //     .save("/home/user/dumps/imageZ_interpolated.png");

            auto rawProfiles = openDump("/home/user/dumps/binx");
            qDebug() << "raw x-profiles count is" << rawProfiles.size();
            // qDebug() << "height" << calibrationColumnHeight;

            auto filteredRawProfiles = filter(std::move(rawProfiles));
            qDebug() << "filtered x-profiles count is"
                     << filteredRawProfiles.count();

            ::calibrationTableX = calibrateX(std::move(filteredRawProfiles));

            // for (size_t i = 9471; i < 9472; i++) {
            //     std::cout << "row #" << i << ": ";

            //     for (size_t j = 0; j < 1280; ++j) {
            //         const auto& p = ::calibrationTableX->at(j).at(i);
            //         std::cout << p << ' ';
            //     }

            //     std::cout << std::endl;
            // }

            // x
            // qDebug() << "open x table";
            // if (!openCalibrationTable("/home/user/dumps/binx.calibration_table",
            //                           ::calibrationTableX)) {
            //     exit(EXIT_FAILURE);
            // }

            // if (!calibrationTableToImage(::calibrationTableX)
            //          .save("/home/user/dumps/imageX.png")) {
            //     qDebug() << "cannot save imageX.png";
            //     exit(EXIT_FAILURE);
            // }

            // for (size_t i = 9471; i < 9472; i++) {
            //     std::cout << "row #" << i << ": ";

            //     for (size_t j = 0; j < 1280; ++j) {
            //         const auto& p = ::calibrationTableX->at(j).at(i);
            //         std::cout << p << ' ';
            //     }

            //     std::cout << std::endl;
            // }

            // exit(EXIT_SUCCESS);
            interpolate(::calibrationTableX);

            // calibrationTableToImage(::calibrationTableX)
            //     .save("/home/user/dumps/imageX_interpolated.png");
        }

        // load binary calibration dumps and calibrate
        if (false)
        {
            if (true)
            {
                auto rawProfiles = openDump("/home/user/dumps/binz");
                // auto rawProfiles = openDump("/home/user/dumps/z");
                qDebug() << "raw z-profiles count is" << rawProfiles.size();
                // qDebug() << "height" << calibrationColumnHeight;

                auto filteredRawProfiles = filter(std::move(rawProfiles));
                qDebug() << "filtered z-profiles count is"
                         << filteredRawProfiles.count();

                ::calibrationTableZ = calibrateZ(std::move(filteredRawProfiles),
                                                 requested_params.stepsPerMm);

                // bool ok = calibrationTableToImage(::calibrationTableZ)
                //               .save("/home/user/dumps/z/imageZ.png");

                // if (!ok)
                // {
                //     qDebug() << "cannot save imageZ.png";
                //     exit(EXIT_FAILURE);
                // }

                interpolate(::calibrationTableZ);

                if (!dump(::calibrationTableZ,
                          "/home/user/dumps/binz.calibration_table"))
                {
                    qApp->exit(EXIT_FAILURE);
                }
                // calibrationTableToImage(::calibrationTableZ)
                //     .save("/home/user/dumps/z/imageZ_interpolated.png");
                // exit(EXIT_SUCCESS);
            }

            qDebug()
                << "--------------------------------------------------------";

            if (true)
            {
                auto rawProfiles = openDump("/home/user/dumps/binx");
                qDebug() << "raw x-profiles count is" << rawProfiles.size();
                // qDebug() << "height" << calibrationColumnHeight;

                auto filteredRawProfiles = filter(std::move(rawProfiles));
                qDebug() << "filtered x-profiles count is"
                         << filteredRawProfiles.count();

                ::calibrationTableX = calibrateX(std::move(filteredRawProfiles));

                // bool ok = calibrationTableToImage(::calibrationTableX)
                //               .save("/home/user/dumps/z/imageX.png");

                // if (!ok)
                // {
                //     qDebug() << "cannot save imageX.png";
                //     exit(EXIT_FAILURE);
                // }

                interpolate(::calibrationTableX);

                if (!dump(::calibrationTableX,
                          "/home/user/dumps/binx.calibration_table"))
                {
                    qApp->exit(EXIT_FAILURE);
                }

                // calibrationTableToImage(::calibrationTableX)
                //     .save("/home/user/dumps/z/imageX_interpolated.png");
            }
        }
    }

    // exit(EXIT_SUCCESS);

    // if (!initLaser()) {
    //     return EXIT_FAILURE;
    // }

    // PrinterClient printerClient;

    QElapsedTimer t;
    t.start();

    qDebug() << "msecs before encoder:" << t.elapsed();

    RotaryEncoder encoder;

    qDebug() << "msecs before camera:" << t.elapsed();
    // FIXME: don't use one var for everything
    int ret;
#ifndef INNO_MAKER
    std::unique_ptr<libcamera::CameraManager> cm =
        std::make_unique<libcamera::CameraManager>();
    cm->start();
#endif
    // const auto cameras = cm->cameras();
    // const auto cameras = OV9281::search(cm);
    const auto cameras = InnoMakerOV9281::search();
    // const auto cameras =

    if (cameras.empty())
    {
        std::cerr << "No cameras were identified on the system." << std::endl;
#ifndef INNO_MAKER
        cm->stop();
#endif

        return EXIT_FAILURE;
    }

    auto camera = cameras.at(0);

#ifndef INNO_MAKER
    camera->printControls();
#endif

    std::cout << "connect everything" << std::endl;
    camera->newPixels.connect(&onNewPixels);
    camera->newImage.connect(&onNewImage);
    camera->newImageCallback = &onNewImage;
    camera->newPixelsCallback = &onNewPixels;

    for (auto& i : initializers)
        i.waitForFinished();

    if (!camera->startStream())
    {
#ifndef INNO_MAKER
        cm->stop();
#endif

        return EXIT_FAILURE;
    }

    QHttpServer qHttpServer;
    qHttpServer.route("/v1/sensor/image", [&]() {
        // std::cout << "http: image" << std::endl << std::flush;
        // FILE *f = fopen("/tmp/img.pgm", "w");
        // static bool save = false;
        pgm_save(::img);
        // save = false;
        std::lock_guard<std::mutex> lg(pgm_image_mtx);
        // qDebug() << "mutex locked";
        // qDebug() << "image saved to array";
        return QByteArray((const char*) pgm_image, pgm_image_size);
    });
    qHttpServer.route("/v1/sensor/image2", [&]() {
        // std::cout << "http: image2" << std::endl;
        pgm_save(::img);

        std::lock_guard<std::mutex> lg(pgm_image_mtx);
        // qDebug() << "image2";
        return QByteArray((const char*) pgm_image, pgm_image_size);
    });
    // qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() {
    //     // std::lock_guard<std::mutex> lg(pgm_image_mtx);
    //     return "123";
    // });
    qHttpServer.route("/v1/pixels", [&]() {
        // std::cout << "http: pixels" << std::endl;
        std::lock_guard<std::mutex> lg(pgm_image_mtx);

        QJsonArray pixels;

        for (size_t i = 0; i < img_width; ++i)
        {
            // pixels << img_height - img.pixels[i];
            pixels << ::pixels.pixels[i];
        }

        QJsonObject json;
        json["pixels"] = pixels;
        json["encoderPosition"] = qint64{encoder.position()};
        // FIXME: get prom pixels struct
        json["measurementCounter"] = qint64{img->counters.measurementCounter};
        json["timestampUs"] = qint64(img->counters.timestampUs);

        const auto lines = pixelsToLines(::pixels);

        // qDebug() << "lines count is " << lines.count();

        QJsonArray jsonLines;

        for (const auto& l : lines)
        {
            jsonLines << QJsonArray{QJsonArray{l.p1().x(), l.p1().y()},
                                    QJsonArray{l.p2().x(), l.p2().y()}};
        }

        json["lines"] = jsonLines;

        return QHttpServerResponse(QJsonDocument(json).toJson());
    });

    qHttpServer.route("/v1/profile", [&]() -> QHttpServerResponse {
        // std::cout << "http: profile" << std::endl;
        std::lock_guard<std::mutex> lg(pgm_image_mtx);

        if (!::calibrationTableZ || !::calibrationTableX)
            return QHttpServerResponse::StatusCode::ServiceUnavailable;

        const Profile profile(::pixels,
                              ::calibrationTableZ,
                              ::calibrationTableX);

        const QJsonObject json{{"profile", QJsonObject(profile)}};

        return QHttpServerResponse(QJsonDocument(json).toJson());
    });

    qHttpServer
        .route("/v1/commands/resetEncoder",
               [&](const QHttpServerRequest& request) -> QHttpServerResponse {
                   std::cout << "http: resetEncoder" << std::endl;
                   if (request.method() != QHttpServerRequest::Method::Post)
                   {
                       return QHttpServerResponse::StatusCode::NotFound;
                   }

                   qDebug() << "reset encoder";

                   positionSteps = 0;

                   return QHttpServerResponse::StatusCode::Ok;
               });

    qHttpServer
        .route("/v1/commands/startCalibration",
               [&](const QHttpServerRequest& request) -> QHttpServerResponse {
                   std::cout << "http: startCalibration" << std::endl;
                   if (request.method() != QHttpServerRequest::Method::Post)
                   {
                       return QHttpServerResponse::StatusCode::NotFound;
                   }

                   qDebug() << "start calibration";

                   // TODO: use flags
                   scanningModeFlags = ScanningModeFlags::Calibration;
                   calibrationTimer.start();

                   return QHttpServerResponse::StatusCode::Ok;
               });

    qHttpServer
        .route("/v1/commands/gCode",
               [&](const QHttpServerRequest& request) -> QHttpServerResponse {
                   std::cout << "http: gCode" << std::endl;
                   if (request.method() != QHttpServerRequest::Method::Post)
                   {
                       return QHttpServerResponse::StatusCode::NotFound;
                   }

                   const auto command = request.body();

                   qDebug() << "send gCode:" << command;

                   // printerClient.sendCommand(command);

                   return QHttpServerResponse::StatusCode::Ok;
               });

    // qHttpServer
    //     .route("/v1/commands/startCalibration",
    //            [&](const QHttpServerRequest& request) -> QHttpServerResponse {
    //                std::cout << "http: startCalibration" << std::endl;
    //                if (request.method() != QHttpServerRequest::Method::Post)
    //                {
    //                    return QHttpServerResponse::StatusCode::NotFound;
    //                }

    //                const auto command = request.body();

    //                qDebug() << "send gCode:" << command;

    //                // printerClient.sendCommand(command);

    //                return QHttpServerResponse::StatusCode::Ok;
    //            });

    qHttpServer.route(
        "/v1/sensor/params",
        [&](const QHttpServerRequest& request) -> QHttpServerResponse {
            std::cout << "http: params" << std::endl;
            switch (request.method())
            {
            case QHttpServerRequest::Method::Get: {
                std::lock_guard<std::mutex> lg(pgm_image_mtx);
                QJsonObject json;

                // const libcamera::ControlIdMap& ctrlIdMap =
                //     camera->controls().idmap();

                // qDebug() << "readParams:" << lastControls.size();
                // qDebug() << request.method();

                // for (const auto& [id, value] : lastControls)
                // {
                //     const libcamera::ControlId* controlId = ctrlIdMap.at(id);
                //     auto name = QString::fromStdString(controlId->name());
                //     const auto valueStr =
                //         QString::fromStdString(value.toString());
                //     qDebug()
                //         << "\t param:" << controlId->id() << name << valueStr;

                //     name[0] = name[0].toLower();
                //     json[name] = valueStr;
                // }

                // json[laserLevelKey] = requested_params.laserLevel;

                // qDebug() << "response body:" << json;

                // QHttpServerResponse
                return QHttpServerResponse(QJsonDocument(json).toJson());
            }

            case QHttpServerRequest::Method::Post: {
                qDebug() << "request body:" << request.body();

                auto json = QJsonDocument::fromJson(request.body()).object();

                if (json.contains(exposureTimeKey))
                {
                    const int32_t value{json[exposureTimeKey].toInt()};

                    if (value == 0)
                        return QHttpServerResponse::StatusCode::
                            RequestRangeNotSatisfiable;

                    qDebug() << "set new exposure time:" << value;

                    // requested_params.exposureTime = value;
                    if (!camera->setExposureTimeUs(value))
                        return QHttpServerResponse::StatusCode::
                            RequestRangeNotSatisfiable;
                }

                if (json.contains(laserLevelKey))
                {
                    const int32_t value{json[laserLevelKey].toInt()};

                    if (value == 0)
                    {
                        return QHttpServerResponse::StatusCode::NotFound;
                    }

                    qDebug() << "set new laserLevel:" << value;
                    requested_params.laserLevel = value;

                    const QString laserLevelFile{
                        "/sys/class/pwm/pwmchip2/pwm1/duty_cycle"};
                    QFile f{laserLevelFile};

                    if (!f.open(QFile::ReadWrite))
                    {
                        qDebug() << "cannot open laser level file:"
                                 << f.errorString();
                        qDebug() << "file path is" << f.fileName();
                        return QHttpServerResponse::StatusCode::InternalServerError;
                    }

                    QTextStream s{&f};

                    s << value;

                    s >> requested_params.laserLevel;

                    qDebug() << "done with laser level";
                }

                return QHttpServerResponse(request.body());
            }
            default: {
                return QHttpServerResponse(
                    QByteArray("unsupported http method"));
            }
            }
        });

    qDebug() << "listen: " << qHttpServer.listen(QHostAddress::Any, 8081);

    QFuture<void> future = QtConcurrent::run([]() {
        Port port(8080);
        Address addr(Ipv4::any(), port);

        HttpService httpService(addr);

        size_t threads_count = 1;
        httpService.init(threads_count);
        httpService.start();
    });

    ////////////////////////////////////////////////////////////////////////////
    std::clog << std::flush;
    std::cerr << std::flush;
    std::cout << "ok for now" << std::endl << std::flush;

    // camera->stop();
    // camera->release();
    // cm->stop();

    auto result = app.exec();

    future.cancel();
    future.waitForFinished();

    // for (auto& [fd, mem] : mappedBuffers_)
    // {
    //     munmap(mem.first, mem.second);
    // }

    // FIXME: crash somewhere here. proper libcamera finishing needed
    // requests.clear();
    // mappedBuffers_.clear();

    // camera->stop();
    // config.reset();
    // allocator->free(stream);
    // allocator.reset();
    // camera->release();
    // camera.reset();
#ifndef INNO_MAKER
    cm->stop();
#endif

    return result;
}

bool initLaser()
{
    const QLatin1String pwmChip{"pwmchip2"};
    const uint16_t pwmChannel{1};
    const QLatin1String pwmSystemRoot{"/sys/class/pwm"};
    const QString pwmChipRoot{pwmSystemRoot + "/" + pwmChip};

    const QString pwmExportFile{pwmChipRoot + "/export"};

    QFile f{pwmExportFile};

    if (!f.open(QFile::WriteOnly))
    {
        qWarning() << "cannot open" << f.fileName() << "for writing";
        qWarning() << "error:" << f.errorString();

        return false;
    }

    QTextStream s{&f};
    s << pwmChannel;

    const QString pwm{QLatin1String("pwm%1").arg(QString::number(pwmChannel))};
    const QString pwmRoot{pwmChipRoot + "/" + pwm};

    const QString periodFilename{pwmRoot + "/period"};
    f.close();
    f.setFileName(periodFilename);

    if (!f.open(QFile::WriteOnly))
    {
        qWarning() << "cannot open" << f.fileName() << "for writing";
        qWarning() << "error:" << f.errorString();

        return false;
    }

    const unsigned periodHz{50'000};

    s << periodHz;

    const QString dutyCycleFilename{pwmRoot + "/duty_cycle"};
    f.close();
    f.setFileName(dutyCycleFilename);

    if (!f.open(QFile::WriteOnly))
    {
        qWarning() << "cannot open" << f.fileName() << "for writing";
        qWarning() << "error:" << f.errorString();

        return false;
    }

    const unsigned dutyCycle{3'000};

    s << dutyCycle;

    const QString enableFilename{pwmRoot + "/enable"};
    f.close();
    f.setFileName(enableFilename);

    if (!f.open(QFile::WriteOnly))
    {
        qWarning() << "cannot open" << f.fileName() << "for writing";
        qWarning() << "error:" << f.errorString();

        return false;
    }

    const int enable{1};

    s << enable;

    return true;
}