main.cpp

/*
 * Copyright 2019 Nicolas Pope. All rights reserved.
 *
 * See LICENSE.
 */

#define LOGURU_WITH_STREAMS 1
#include <loguru.hpp>
#include <ftl/configuration.hpp>
#include <ctpl_stl.h>

#include <string>
#include <map>
#include <vector>
#include <fstream>
#include <thread>

#include <opencv2/opencv.hpp>
#include <ftl/rgbd.hpp>
#include <ftl/data/framepool.hpp>
#include <ftl/streams/builder.hpp>
//#include <ftl/middlebury.hpp>
#include <ftl/net/universe.hpp>
#include <ftl/master.hpp>
#include <nlohmann/json.hpp>
#include <ftl/operators/disparity.hpp>
#include <ftl/operators/detectandtrack.hpp>

#include <ftl/streams/netstream.hpp>
#include <ftl/streams/sender.hpp>
#include <ftl/streams/receiver.hpp>

#include <ftl/audio/source.hpp>

#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"

#ifdef HAVE_PYLON
#include <pylon/PylonIncludes.h>
#endif

#ifdef WIN32
#pragma comment(lib, "Rpcrt4.lib")
#endif

using ftl::rgbd::Source;
using ftl::rgbd::Camera;
using ftl::codecs::Channel;
using ftl::net::Universe;
using std::string;
using std::vector;
using std::map;
using std::condition_variable;
using std::this_thread::sleep_for;
using std::chrono::milliseconds;
using cv::Mat;
using json = nlohmann::json;

static void run(ftl::Configurable *root) {
	Universe *net = ftl::create<Universe>(root, "net");

	ftl::timer::setHighPrecision(true);

	if (root->value("time_master", false)) {
		net->bind("time_master", [net]() {
			return net->id();
		});
		LOG(INFO) << "Becoming a time master";
	}

	if (root->get<string>("time_peer")) {
		if (!net->connect(*root->get<string>("time_peer"))->waitConnection()) {
			LOG(ERROR) << "Could not connect to time master";
		} else {
			LOG(INFO) << "Connected to time master";
		}
	}

	auto opt_time_master = net->findOne<ftl::UUID>(string("time_master"));
	ftl::UUID time_peer(0);
	if (opt_time_master) {
		time_peer = *opt_time_master;
		LOG(INFO) << "Found a time master: " << time_peer.to_string();
	}
	int sync_counter = 0;

	ftl::ctrl::Master ctrl(root, net);

	// Sync clocks!
	auto timer = ftl::timer::add(ftl::timer::kTimerMain, [&time_peer,&sync_counter,net](int64_t ts) {
		if (sync_counter-- <= 0 && time_peer != ftl::UUID(0) ) {
			sync_counter = 20;
			auto start = std::chrono::high_resolution_clock::now();

			try {
				net->asyncCall<int64_t>(time_peer, "__ping__", [start](const int64_t &mastertime) {
					auto elapsed = std::chrono::high_resolution_clock::now() - start;
					int64_t latency = std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count();
					auto clock_adjust = mastertime + ((latency+500)/2000) - ftl::timer::get_time();

					//LOG(INFO) << "LATENCY: " << float(latency)/1000.0f << "ms";

					if (clock_adjust != 0) {
						LOG(INFO) << "Clock adjustment: " << clock_adjust << ", latency=" << float(latency)/1000.0f << "ms";
						ftl::timer::setClockAdjustment(clock_adjust);
					}		
				});
			} catch (const std::exception &e) {
				LOG(ERROR) << "Ping failed, could not time sync: " << e.what();
				return true;
			}
		}
		return true;
	});

	auto paths = root->get<vector<string>>("paths");
	string file = "";
	//if (paths && (*paths).size() > 0) file = (*paths)[(*paths).size()-1];

	for (auto &x : *paths) {
		//LOG(INFO) << "PATH - " << x;
		if (x != "") {
			ftl::URI uri(x);
			if (uri.isValid()) {
				switch (uri.getScheme()) {
				case ftl::URI::SCHEME_WS		:
				case ftl::URI::SCHEME_TCP		: net->connect(x)->waitConnection(); break;
				case ftl::URI::SCHEME_DEVICE	:
				case ftl::URI::SCHEME_FILE		: file = x; break;
				default: break;
				}
			}
		}
	}

	Source *source = nullptr;
	source = ftl::create<Source>(root, "source", net);
	if (file != "") {
		//source->set("uri", file);
		ftl::URI uri(file);
		uri.to_json(source->getConfig());
		source->set("uri", uri.getBaseURI());
	}
	
	ftl::stream::Sender *sender = ftl::create<ftl::stream::Sender>(root, "sender");
	ftl::stream::Net *outstream = ftl::create<ftl::stream::Net>(root, "stream", net);
	outstream->set("uri", outstream->getID());
	outstream->begin();
	sender->setStream(outstream);

	ftl::audio::Source *audioSrc = ftl::create<ftl::audio::Source>(root, "audio_test");

	ftl::data::Pool pool(2,5);
	auto *creator = new ftl::streams::IntervalSourceBuilder(&pool, 0, {source, audioSrc});
	std::shared_ptr<ftl::streams::BaseBuilder> creatorptr(creator);

	ftl::stream::Receiver *receiver = ftl::create<ftl::stream::Receiver>(root, "receiver", &pool);
	receiver->setStream(outstream);
	receiver->registerBuilder(creatorptr);

	// Send channels on flush
	auto flushhandle = pool.onFlushSet([sender](ftl::data::FrameSet &fs, ftl::codecs::Channel c) {
		// TODO: Check the channel to see if it should be sent or not
		switch (c) {
		case Channel::Colour	:
		case Channel::Colour2	:
		case Channel::Depth		: sender->post(fs, c); break;
		default: break;
		}
		return true;
	});

	int stats_count = 0;
	int frames = 0;
	float latency = 0.0f;

	auto *pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline");
	pipeline->append<ftl::operators::DetectAndTrack>("facedetection")->value("enabled", false);
	pipeline->append<ftl::operators::ArUco>("aruco")->value("enabled", false);
	pipeline->append<ftl::operators::DepthChannel>("depth");  // Ensure there is a depth channel

	bool busy = false;

	auto h = creator->onFrameSet([sender,&stats_count,&latency,&frames,pipeline,&busy](const ftl::data::FrameSetPtr &fs) {
		if (busy) return true;
		busy = true;

		// TODO: Remove, this is debug code
		if (fs->firstFrame().changed(ftl::codecs::Channel::Control)) {
			LOG(INFO) << "Got control: " << fs->firstFrame().get<int>(ftl::codecs::Channel::Control);
		}

		// Do all processing in another thread...
		ftl::pool.push([sender,&stats_count,&latency,&frames,pipeline,&busy,fs](int id) {
			// Do pipeline here...
			pipeline->apply(*fs, *fs);

			// Send any remaining channels...
			// Note: Ensures these send now, otherwise waits until destructor
			//fs->flush();

			++frames;
			latency += float(ftl::timer::get_time() - fs->timestamp());

			if (--stats_count <= 0) {
				latency /= float(frames);
				LOG(INFO) << "Frame rate: " << frames << ", Latency: " << latency;
				stats_count = 20;
				frames = 0;
				latency = 0.0f;
			}

			busy = false;
		});

		// Lock colour right now to encode in parallel
		fs->flush(ftl::codecs::Channel::Colour);
		fs->flush(ftl::codecs::Channel::AudioStereo);

		return true;
	});

	// Start the timed generation of frames
	creator->start();

	// TODO: TEMPORARY
	/*ftl::audio::Source *audioSrc = ftl::create<ftl::audio::Source>(root, "audio_test");
	audioSrc->onFrameSet([sender](ftl::audio::FrameSet &fs) {
		sender->post(fs);
		return true;
	}); */
	
	net->start();

	LOG(INFO) << "Running...";
	ftl::timer::start(true);
	LOG(INFO) << "Stopping...";
	ctrl.stop();
	
	net->shutdown();

	ftl::pool.stop();

	delete outstream;

	//delete source;  // TODO(Nick) Add ftl::destroy
	delete net;
}

static void threadSetCUDADevice() {
	// Ensure all threads have correct cuda device
	std::atomic<int> ijobs = 0;
	for (int i=0; i<ftl::pool.size(); ++i) {
		ftl::pool.push([&ijobs](int id) {
			ftl::cuda::setDevice();
			++ijobs;
			while (ijobs < ftl::pool.size()) std::this_thread::sleep_for(std::chrono::milliseconds(10));
		});
	}
	while (ijobs < ftl::pool.size()) std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

int main(int argc, char **argv) {
#ifdef HAVE_PYLON
	Pylon::PylonAutoInitTerm autoInitTerm;
#endif

#ifdef WIN32
	SetPriorityClass(GetCurrentProcess(), HIGH_PRIORITY_CLASS);
#endif
	std::cout << "FTL Vision Node " << FTL_VERSION_LONG << std::endl;
	auto root = ftl::configure(argc, argv, "vision_default");

	// Use other GPU if available.
	//ftl::cuda::setDevice(ftl::cuda::deviceCount()-1);
	
	std::cout << "Loading..." << std::endl;
	run(root);

	delete root;
	LOG(INFO) << "Terminating with code " << ftl::exit_code;
	LOG(INFO) << "Branch: " << ftl::branch_name;
	return ftl::exit_code;
}