//=============================================================================================================================
//
// 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.
//
//=============================================================================================================================

using System;
using System.Collections.Generic;
using System.Linq;
using easyar;

namespace HelloAR
{
    public class HelloAR : IDisposable
    {
        private DelayedCallbackScheduler scheduler;
        private CameraDevice camera;
        private ImageTracker tracker;
        private Dictionary<int, ImageTarget> trackedTargets = new Dictionary<int, ImageTarget>();
        private InputFrameThrottler throttler;
        private OutputFrameFork outputFrameFork;
        private OutputFrameBuffer outputFrameBuffer;
        private InputFrameToFeedbackFrameAdapter i2FAdapter;

        private void loadFromImage(ImageTracker tracker, string path, string name)
        {
            var imageTarget = ImageTarget.createFromImageFile(path, StorageType.Assets, name, "", "", 1f);

            if (imageTarget.OnNone)
            {
                Console.WriteLine("create {0} imagetarget failed", name);
                return;
            }

            tracker.loadTarget(imageTarget.Value, scheduler, (t, status) =>
            {
                Console.WriteLine("load target ({0}): {1} ({2})", status, t.name(), t.runtimeID());
            });
        }

        private void initialize()
        {
            scheduler = new DelayedCallbackScheduler();
            camera = CameraDeviceSelector.createCameraDevice(CameraDevicePreference.PreferObjectSensing);
            throttler = InputFrameThrottler.create();
            i2FAdapter = InputFrameToFeedbackFrameAdapter.create();
            outputFrameFork = OutputFrameFork.create(2);
            outputFrameBuffer = OutputFrameBuffer.create();

            if (!camera.openWithPreferredType(CameraDeviceType.Back)) { return; }
            camera.setSize(new Vec2I(1280, 960));
            camera.setBufferCapacity(10);

            tracker = 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");

            using (var ar = new AutoRelease())
            {
                camera.inputFrameSource().AddToAutoRelease(ar).connect(throttler.input().AddToAutoRelease(ar));
                throttler.output().AddToAutoRelease(ar).connect(i2FAdapter.input().AddToAutoRelease(ar));
                i2FAdapter.output().AddToAutoRelease(ar).connect(tracker.feedbackFrameSink().AddToAutoRelease(ar));
                tracker.outputFrameSource().AddToAutoRelease(ar).connect(outputFrameFork.input().AddToAutoRelease(ar));
                outputFrameFork.output(0).AddToAutoRelease(ar).connect(outputFrameBuffer.input().AddToAutoRelease(ar));

                outputFrameBuffer.signalOutput().AddToAutoRelease(ar).connect(throttler.signalInput().AddToAutoRelease(ar));
                outputFrameFork.output(1).AddToAutoRelease(ar).connect(i2FAdapter.sideInput().AddToAutoRelease(ar));
            }
        }

        public HelloAR()
        {
            initialize();
        }

        public void Dispose()
        {
            foreach (var t in trackedTargets)
            {
                t.Value.Dispose();
            }
            stop();

            trackedTargets.Clear();
            tracker.Dispose();
            camera.Dispose();

            outputFrameBuffer.Dispose();
            outputFrameFork.Dispose();
            i2FAdapter.Dispose();
            throttler.Dispose();

            scheduler.Dispose();
        }

        public bool start()
        {
            bool status = true;
            status &= camera.start();
            camera.setFocusMode(CameraDeviceFocusMode.Continousauto);
            tracker.start();
            return status;
        }

        public void stop()
        {
            camera.stop();
            tracker.stop();
            foreach (var t in trackedTargets)
            {
                t.Value.Dispose();
            }
            trackedTargets.Clear();
        }

        public void nextFrame()
        {
            using (var ar = new AutoRelease())
            {
                while (scheduler.runOne())
                {
                }

                var oFrame = outputFrameBuffer.peek();
                if (oFrame.OnNone)
                {
                    return;
                }
                var frame = oFrame.Value.AddToAutoRelease(ar);
                Console.WriteLine("frame: {0}", frame.index());
                var lostCandidates = new Dictionary<int, ImageTarget>(trackedTargets);

                var results = frame.results().AddToAutoRelease(ar);
                foreach (var result in results)
                {
                    if (result.OnNone) { continue; }
                    var imageTrackerResult = result.Value as ImageTrackerResult;
                    foreach (var targetInstance in imageTrackerResult.targetInstances().AddToAutoRelease(ar))
                    {
                        var status = targetInstance.status();
                        var target = targetInstance.target().AddToAutoRelease(ar);
                        var imagetarget = target as ImageTarget;
                        if (imagetarget == null) { continue; }
                        var runtimeID = imagetarget.runtimeID();
                        if (!trackedTargets.ContainsKey(runtimeID))
                        {
                            trackedTargets.Add(runtimeID, imagetarget.Clone());
                            Console.WriteLine("found target: {0} ({1})", imagetarget.name(), runtimeID);
                        }
                        if (lostCandidates.ContainsKey(runtimeID))
                        {
                            lostCandidates.Remove(runtimeID);
                        }
                        var pose = targetInstance.pose();
                        Console.WriteLine("target: {0} ({1}) {2}", imagetarget.name(), runtimeID, String.Join(" ", pose.data.Select(v => v.ToString())));
                    }
                }
                foreach (var p in lostCandidates)
                {
                    var imagetarget = p.Value;
                    var runtimeID = imagetarget.runtimeID();
                    if (trackedTargets.ContainsKey(runtimeID))
                    {
                        trackedTargets.Remove(runtimeID);
                        Console.WriteLine("lost target: {0} ({1})", imagetarget.name(), runtimeID);
                        imagetarget.Dispose();
                    }
                }
            }
        }
    }

    public class AutoRelease : IDisposable
    {
        private List<Action> actions;

        public void Add(Action deleter)
        {
            if (actions == null) { actions = new List<Action>(); }
            actions.Add(deleter);
        }
        public T Add<T>(T p, Action<T> deleter)
        {
            if (p.Equals(default(T))) { return p; }
            if (actions == null) { actions = new List<Action>(); }
            actions.Add(() => deleter(p));
            return p;
        }

        public void Dispose()
        {
            if (actions != null)
            {
                foreach (var a in actions)
                {
                    a();
                }
                actions = null;
            }
        }
    }

    public static class AutoReleaseUtil
    {
        public static T AddToAutoRelease<T>(this T t, AutoRelease ar) where T : IDisposable
        {
            if (t == null) { return default(T); }
            ar.Add(() => t.Dispose());
            return t;
        }
        public static Optional<T> AddToAutoRelease<T>(this Optional<T> ot, AutoRelease ar) where T : IDisposable
        {
            if (ot.OnNone) { return Optional<T>.Empty; }
            var t = ot.Value;
            ar.Add(() => t.Dispose());
            return t;
        }
        public static IEnumerable<T> AddToAutoRelease<T>(this IEnumerable<T> e, AutoRelease ar) where T : IDisposable
        {
            foreach (var t in e)
            {
                if (t == null) { continue; }
                ar.Add(() => t.Dispose());
            }
            return e;
        }
        public static IEnumerable<Optional<T>> AddToAutoRelease<T>(this IEnumerable<Optional<T>> e, AutoRelease ar) where T : IDisposable
        {
            foreach (var ot in e)
            {
                if (ot.OnNone) { continue; }
                var t = ot.Value;
                ar.Add(() => t.Dispose());
            }
            return e;
        }
    }
}

