//=============================================================================================================================
//
// Copyright (c) 2015-2022 VisionStar Information Technology (Shanghai) Co., Ltd. All Rights Reserved.
// EasyAR is the registered trademark or trademark of VisionStar Information Technology (Shanghai) Co., Ltd in China
// and other countries for the augmented reality technology developed by VisionStar Information Technology (Shanghai) Co., Ltd.
//
//=============================================================================================================================

#include "helloar.hpp"

#include "easyar/types.hpp"

#include <cmath>
#include <string>
#include <algorithm>
#include <memory>
#include <vector>
#include <unordered_map>
#include <cstdio>
#include <time.h>
#include <iostream>

std::shared_ptr<easyar::DelayedCallbackScheduler> scheduler;
std::shared_ptr<easyar::CameraDevice> camera;
std::shared_ptr<easyar::ImageTracker> tracker;
std::unordered_map<int, std::shared_ptr<easyar::ImageTarget>> trackedTargets;
std::shared_ptr<easyar::InputFrameThrottler> throttler;
std::shared_ptr<easyar::OutputFrameFork> outputFrameFork;
std::shared_ptr<easyar::OutputFrameBuffer> outputFrameBuffer;
std::shared_ptr<easyar::InputFrameToFeedbackFrameAdapter> i2FAdapter;

void loadFromImage(std::shared_ptr<easyar::ImageTracker> tracker, const std::string & path, const std::string & name)
{
    std::optional<std::shared_ptr<easyar::ImageTarget>> imageTarget = easyar::ImageTarget::createFromImageFile(path, easyar::StorageType::Assets, name, "", "", 1.0f);
    if (imageTarget.has_value())
    {
        tracker->loadTarget(imageTarget.value(), scheduler, [](std::shared_ptr<easyar::Target> target, bool status) {
            std::printf("load target (%d): %s (%d)\n", status, target->name().c_str(), target->runtimeID());
        });
    }
}

void initialize()
{
    scheduler = std::make_shared<easyar::DelayedCallbackScheduler>();

    throttler = easyar::InputFrameThrottler::create();
    i2FAdapter = easyar::InputFrameToFeedbackFrameAdapter::create();
    outputFrameFork = easyar::OutputFrameFork::create(2);
    outputFrameBuffer = easyar::OutputFrameBuffer::create();

    camera = easyar::CameraDeviceSelector::createCameraDevice(easyar::CameraDevicePreference::PreferObjectSensing);
    if (!camera->openWithPreferredType(easyar::CameraDeviceType::Back)) { return; }
    camera->setFocusMode(easyar::CameraDeviceFocusMode::Continousauto);
    camera->setSize(easyar::Vec2I{{1280, 960}});
    camera->setBufferCapacity(10);
    camera->setStateChangedCallback(scheduler, [](easyar::CameraState s)
    {
        if (s == easyar::CameraState::Disconnected)
        {
            std::printf("camera disconnected");
        }
        else if (s == easyar::CameraState::Preempted)
        {
            std::printf("camera preempted");
        }
    });

    tracker = easyar::ImageTracker::create();
    loadFromImage(tracker, "sightplus/argame00.jpg", "argame00");
    loadFromImage(tracker, "sightplus/argame01.jpg", "argame01");
    loadFromImage(tracker, "sightplus/argame02.jpg", "argame02");
    loadFromImage(tracker, "sightplus/argame03.jpg", "argame03");
    loadFromImage(tracker, "idback.jpg", "idback");
    loadFromImage(tracker, "namecard.jpg", "namecard");

    camera->inputFrameSource()->connect(throttler->input());
    throttler->output()->connect(i2FAdapter->input());
    i2FAdapter->output()->connect(tracker->feedbackFrameSink());
    tracker->outputFrameSource()->connect(outputFrameFork->input());
    outputFrameFork->output(0)->connect(outputFrameBuffer->input());

    outputFrameBuffer->signalOutput()->connect(throttler->signalInput());
    outputFrameFork->output(1)->connect(i2FAdapter->sideInput());
}

void finalize()
{
    trackedTargets.clear();
    tracker = nullptr;
    camera = nullptr;
    throttler = nullptr;
    outputFrameFork = nullptr;
    outputFrameBuffer = nullptr;
    i2FAdapter = nullptr;
    scheduler = nullptr;
}

bool start()
{
    bool status = true;
    status &= camera->start();
    status &= tracker->start();
    return status;
}

void stop()
{
    tracker->stop();
    camera->stop();
}

void nextFrame()
{
    while (scheduler->runOne()) {}

    auto oFrame = outputFrameBuffer->peek();
    if (!oFrame.has_value()) { return; }
    auto frame = oFrame.value();
    auto lostCandidates = trackedTargets;
    for (auto && result : frame->results())
    {
        if (!result.has_value()) { continue; }
        auto imageTrackerResult = std::dynamic_pointer_cast<easyar::ImageTrackerResult>(result.value());
        if (imageTrackerResult != nullptr)
        {
            for (auto && targetInstance : imageTrackerResult->targetInstances())
            {
                auto status = targetInstance->status();
                auto target = targetInstance->target();
                auto imagetarget = std::dynamic_pointer_cast<easyar::ImageTarget>(target);
                if (imagetarget == nullptr)
                {
                    continue;
                }
                if (trackedTargets.count(imagetarget->runtimeID()) == 0)
                {
                    trackedTargets[imagetarget->runtimeID()] = imagetarget;
                    std::printf("found target: %s (%d)\n", imagetarget->name().c_str(), imagetarget->runtimeID());
                }
                lostCandidates.erase(imagetarget->runtimeID());

                auto pose = targetInstance->pose();
                std::printf("target: %s (%d) %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", imagetarget->name().c_str(), imagetarget->runtimeID(), pose.data[0], pose.data[1], pose.data[2], pose.data[3], pose.data[4], pose.data[5], pose.data[6], pose.data[7], pose.data[8], pose.data[9], pose.data[10], pose.data[11], pose.data[12], pose.data[13], pose.data[14], pose.data[15]);
            }
        }
    }

    for (auto p : lostCandidates)
    {
        auto imagetarget = std::get<1>(p);
        if (trackedTargets.count(imagetarget->runtimeID()) > 0)
        {
            trackedTargets.erase(imagetarget->runtimeID());
            std::printf("lost target: %s (%d)\n", imagetarget->name().c_str(), imagetarget->runtimeID());
        }
    }
}
