pip/src_main/thread/pitimer.cpp

/*
    PIP - Platform Independent Primitives
    Timer
    Copyright (C) 2019  Ivan Pelipenko peri4ko@yandex.ru

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "pitimer.h"
#include <stdio.h>
#ifdef PIP_TIMER_RT
#  include <csignal>
#endif


/*! \class PITimer
 * \brief Timer
 * 
 * \section PITimer_sec0 Synopsis
 * This class implements timer function. PIP timers supports 3 way to tick notify,
 * frequency delimiters and time measurements.
 * \section PITimer_sec1 Notify variants
 *  Notify variants:
 * * "slot" - static function with format void func(void * data, int delimiter);
 * * event - \a tickEvent();
 * * virtual function - \a tick().
 * 
 * All these variants are equivalent, use most applicable.
 * \section PITimer_sec2 Frequency delimiters
 * Frequency delimiter is an integer number and "slot" function. If "slot" function is null
 * timer main "slot" will be used. Each delimiter numbers tick timer will be execute
 * delimiters or timer main "slot" function with \b delimiter value = delimiter number.
 * Example: \snippet pitimer.cpp delimiter
 * \section PITimer_sec3 Time measurements
 * PITimer can be used as time measurer. Function \a reset() set time mark to current
 * system time, then functions double elapsed_*() returns time elapsed from this mark.
 * These functions can returns nano-, micro-, milli- and seconds with suffixes "n", "u", "m"
 * and "s"
 * Example: \snippet pitimer.cpp elapsed
*/


_PITimerBase::_PITimerBase() {
	interval_ = deferred_delay = 0.;
	running_ = deferred_ = deferred_mode = false;
	tfunc = 0;
	parent = 0;
}


void _PITimerBase::setInterval(double i) {
	interval_ = i;
	if (isRunning()) {
		piCout << "change interval runtime";
		stop(true);
		start();
	}
}


bool _PITimerBase::start(double interval_ms) {
	if (isRunning()) stop(true);
	deferred_ = false;
	setInterval(interval_ms);
	running_ = startTimer(interval_ms);
	return running_;
}


void _PITimerBase::startDeferred(double interval_ms, PIDateTime start_datetime) {
	if (isRunning()) stop(true);
	deferred_ = true;
	deferred_mode = true;
	deferred_datetime = start_datetime;
	setInterval(interval_ms);
	running_ = startTimer(interval_ms);
}


void _PITimerBase::startDeferred(double interval_ms, double delay_ms) {
	if (isRunning()) stop(true);
	deferred_ = true;
	deferred_mode = false;
	deferred_delay = delay_ms;
	setInterval(interval_ms);
	running_ = startTimer(interval_ms);
}


bool _PITimerBase::stop(bool wait) {
	piCout << "_PITimerBase::stop" << isRunning();
	if (!isRunning()) return true;
	piCout << "_PITimerBase::stopTimer";
	running_ = !stopTimer(wait);
	return !running_;
}



class _PITimerImp_Thread: public _PITimerBase {
public:
	_PITimerImp_Thread();
	virtual ~_PITimerImp_Thread();
protected:
	void prepareStart(double interval_ms);
	bool threadFunc(); // returns true if repeat is needed
	int wait_dt, wait_dd, wait_tick;
private:
	virtual bool startTimer(double interval_ms);
	virtual bool stopTimer(bool wait);
	static void threadFuncS(void * d) {((_PITimerImp_Thread*)d)->threadFunc();}
	void adjustTimes();

	PIThread thread_;
	PISystemTime st_time, st_inc, st_wait, st_odt;
};


#ifdef PIP_TIMER_RT
struct _PITimerImp_RT_Private_;
class _PITimerImp_RT: public _PITimerBase {
public:
	_PITimerImp_RT();
	virtual ~_PITimerImp_RT();
protected:
private:
	virtual bool startTimer(double interval_ms);
	virtual bool stopTimer(bool wait);

	int ti;
	_PITimerImp_RT_Private_ * priv;
};
#endif


class _PITimerImp_Pool: public _PITimerImp_Thread {
public:
	_PITimerImp_Pool();
	virtual ~_PITimerImp_Pool() {}
private:
	class Pool: public PIThread {
	public:
		static Pool * instance();
		void add(_PITimerImp_Pool * t);
		void remove(_PITimerImp_Pool * t);
		void run();
		PIVector<_PITimerImp_Pool * > timers, to_remove;
	private:
		explicit Pool();
		virtual ~Pool();
	};
	virtual bool startTimer(double interval_ms);
	virtual bool stopTimer(bool wait);
};




_PITimerImp_Thread::_PITimerImp_Thread() {
	thread_.setName("__S__PITimerImp_Thread::thread");
	wait_dt = 100;
	wait_dd = 200;
	wait_tick = 10;
	piCout << "new _PITimerImp_Thread" << &thread_ << this;
}


_PITimerImp_Thread::~_PITimerImp_Thread() {
	piCout << "~_PITimerImp_Thread ..." << &thread_ << this;
	thread_.stop(true);
	piCout << "~_PITimerImp_Thread done" << &thread_ << this;
}


void _PITimerImp_Thread::prepareStart(double interval_ms) {
	if (interval_ms <= 0.) {
		piCout << "Achtung! Start PITimer with interval <= 0!";
		piCout << "Achtung! Parent" << parent;
		assert(interval_ms > 0.);
	}
	st_inc = PISystemTime::fromMilliseconds(interval_ms);
	st_odt = st_inc * 5;
	if (st_odt.toSeconds() < 1.) st_odt = PISystemTime::fromSeconds(1.);
	if (deferred_) {
		if (!deferred_mode)
			st_time = PISystemTime::current(true) + PISystemTime::fromMilliseconds(deferred_delay);
		st_time -= st_inc;
	} else
		st_time = PISystemTime::current(true) + st_inc;
}


bool _PITimerImp_Thread::startTimer(double interval_ms) {
	prepareStart(interval_ms);
	thread_.setData(this);
	return thread_.start(threadFuncS);
}


bool _PITimerImp_Thread::stopTimer(bool wait) {
	piCout << "stop timer..." << &thread_ << this;
#ifndef FREERTOS
	thread_.stop(true);
#else
	thread_.stop();
	if (wait)
		if (!thread_.waitForFinish(10))
			if (thread_.isRunning())
				thread_.terminate();
#endif
	piCout << "stop timer done!" << this << st_wait;
	return true;
}


bool _PITimerImp_Thread::threadFunc() {
	//piCout << "threadFunc";
	//printf("threadFunc\n");
	if (!running_) {
		//piCout << "threadFunc 1";
		//printf("threadFunc 1");
		return false;
	}
	if (deferred_) {
		PISystemTime dwt;
		int wth(wait_dt);
		if (deferred_mode) {
			dwt = deferred_datetime.toSystemTime() - PISystemTime::current();
			wth = wait_dd;
		} else
			dwt = st_time - PISystemTime::current(true);
		if (wth > 0) {
			if (dwt.toMilliseconds() > wth + 1.) {
				//printf("wait 2\n");
				//piCout << "wait 2" << this << dwt;
				msleep(wth);
				//printf("threadFunc 2\n");
				//piCout << "threadFunc 2";
				return false;
			} else {
				//piCout << "wait 3" << this << dwt;
				//printf("wait 3\n");
				dwt.sleep();
				deferred_ = false;
				st_time = PISystemTime::current(true);
			}
		} else {
			if (dwt.toMilliseconds() > 0.1) {
				//piCout << "threadFunc 3";
				//printf("threadFunc 3\n");
				return false;
			}
		}
	}
	st_wait = st_time - PISystemTime::current(true);
	//piCout << "wait" << this << st_wait;
	if (st_wait.abs() > st_odt || st_wait.seconds <= -5) {
		adjustTimes();
		//piCout << "threadFunc 4";
		//printf("threadFunc 4\n");
		return true;
	}
	if (wait_tick > 0) {
		if (st_wait.toMilliseconds() > wait_tick + 1.) {
			//piCout << "wait 5" << this << wait_tick;
			//printf("wait 5 %d\n", wait_tick);
			//fflush(stdout);
			msleep(wait_tick);
			//piCout << "threadFunc 5";
			//printf("threadFunc 5\n");
			//fflush(stdout);
			return false;
		} else {
			//piCout << "wait 6" << this << st_wait;
			//printf("wait 6 %f\n" , st_wait.toMicroseconds());
			st_wait.sleep();
		}
	} else {
		if (st_wait.toMilliseconds() > 0.1) {
			//piCout << "threadFunc 6";
			//printf("threadFunc 6\n");
			return false;
		}
	}
	st_time += st_inc;
	if (!parent->isPIObject()) {
		piCout << "Achtung! PITimer \"parent\" is not PIObject!";
		//piCout << "threadFunc 7";
		printf("threadFunc 7\n");
		return false;
	}
	//piCout << "timer tick";
	//printf("timer tick\n");
	tfunc(parent);
	//piCout << "threadFunc 8";
	//printf("threadFunc 8\n");
	return true;
}


void _PITimerImp_Thread::adjustTimes() {
	PISystemTime cst = PISystemTime::current(true);
	if (st_time < cst) {
		int rs = (cst - st_time).toSeconds() / st_inc.toSeconds();
		if (rs >= 100)
			st_time = cst + st_inc;
		else {
			while (st_time < cst)
				st_time += st_inc;
		}
	} else {
		int rs = (st_time - cst).toSeconds() / st_inc.toSeconds();
		if (rs >= 100)
			st_time = cst - st_inc;
		else {
			cst += st_inc;
			while (st_time > cst)
				st_time -= st_inc;
		}
	}
}




#ifdef PIP_TIMER_RT

void threadFuncS(sigval sv) {((_PITimerImp_RT * )sv.sival_ptr)->tfunc(((_PITimerImp_RT * )sv.sival_ptr)->parent);}

struct _PITimerImp_RT_Private_ {
	itimerspec spec;
	timer_t tt;
	sigevent se;
};

_PITimerImp_RT::_PITimerImp_RT() {
	//piCout << "new _PITimerImp_RT";
	priv = new _PITimerImp_RT_Private_();
	priv->tt = 0;
	ti = -1;
	memset(&(priv->se), 0, sizeof(priv->se));
	priv->se.sigev_notify = SIGEV_THREAD;
	priv->se.sigev_value.sival_ptr = this;
	priv->se.sigev_notify_function = threadFuncS;
	priv->se.sigev_notify_attributes = 0;
}


_PITimerImp_RT::~_PITimerImp_RT() {
	stop(true);
	delete priv;
}


bool _PITimerImp_RT::startTimer(double interval_ms) {
	int flags(0);
	priv->spec.it_interval.tv_nsec = ((int)(interval_ms * 1000) % 1000000) * 1000;
	priv->spec.it_interval.tv_sec = (time_t)(interval_ms / 1000);
	if (deferred_) {
		if (deferred_mode) {
			PISystemTime dtm = deferred_datetime.toSystemTime();
			priv->spec.it_value.tv_nsec = dtm.nanoseconds;
			priv->spec.it_value.tv_sec = dtm.seconds;
			flags = TIMER_ABSTIME;
		} else {
			priv->spec.it_value.tv_nsec = ((int)(deferred_delay * 1000) % 1000000) * 1000;
			priv->spec.it_value.tv_sec = (time_t)(deferred_delay / 1000);
		}
	} else {
		priv->spec.it_value = priv->spec.it_interval;
	}
	ti = timer_create(CLOCK_REALTIME, &(priv->se), &(priv->tt));
	//cout << "***create timer " << msecs << " msecs\n";
	if (ti == -1) {
		piCout << "Can`t create RT timer for " << interval_ms << " msecs: " << errorString();
		return false;
	}
	timer_settime(priv->tt, flags, &(priv->spec), 0);
	return true;
}


bool _PITimerImp_RT::stopTimer(bool wait) {
	if (ti < 0) return true;
	timer_delete(priv->tt);
	ti = -1;
	priv->tt = 0;
	return true;
}

#endif




_PITimerImp_Pool::_PITimerImp_Pool(): _PITimerImp_Thread() {
	wait_dt = wait_dd = wait_tick = 0;
	//piCout << "new _PITimerImp_Pool";
}


_PITimerImp_Pool::Pool::Pool(): PIThread() {
	setName("__S__PITimerImp_Pool::Pool");
	needLockRun(true);
#ifndef FREERTOS
	timers.reserve(64);
	start(PIP_MIN_MSLEEP*5);
#else
	start(PIP_MIN_MSLEEP);
#endif
}


_PITimerImp_Pool::Pool::~Pool() {
	stop();
	if (!waitForFinish(500))
		terminate();
	unlock();
	timers.clear();
}


_PITimerImp_Pool::Pool * _PITimerImp_Pool::Pool::instance() {
	static Pool pool;
	return &pool;
}


void _PITimerImp_Pool::Pool::add(_PITimerImp_Pool * t) {
	//piCout << "add ...";
	lock();
	to_remove.removeAll(t);
	if (!timers.contains(t))
		timers << t;
	unlock();
	//piCout << "add done";
}


void _PITimerImp_Pool::Pool::remove(_PITimerImp_Pool * t) {
	//piCout << "remove ...";
	lock();
	to_remove << t;
	unlock();
	//piCout << "remove done";
}


void _PITimerImp_Pool::Pool::run() {
	if (!to_remove.isEmpty()) {
		piForeach (_PITimerImp_Pool * t, to_remove)
			timers.removeAll(t);
		to_remove.clear();
	}
	piForeach (_PITimerImp_Pool * t, timers)
		t->threadFunc();
}


bool _PITimerImp_Pool::startTimer(double interval_ms) {
	prepareStart(interval_ms);
	Pool::instance()->add(this);
	return true;
}


bool _PITimerImp_Pool::stopTimer(bool wait) {
	Pool::instance()->remove(this);
//	if (wait) {
//		Pool::instance()->waitForDelete(this);
//	}
	return true;
}





PITimer::PITimer(): PIObject() {
	piMonitor.timers++;
#ifdef FREERTOS
	imp_mode = PITimer::Thread;
#else
	imp_mode = PITimer::Thread;
#endif
	initFirst();
}


PITimer::PITimer(PITimer::TimerImplementation ti): PIObject() {
	piMonitor.timers++;
	imp_mode = ti;
	initFirst();
}


PITimer::PITimer(TimerEvent slot, void * data, PITimer::TimerImplementation ti): PIObject() {
	piMonitor.timers++;
	imp_mode = ti;
	initFirst();
	data_t = data;
	ret_func = slot;
}


//PITimer::PITimer(const PITimer & other): PIObject() {
//	piMonitor.timers++;
//	imp_mode = other.imp_mode;
//	initFirst();
//	data_t = other.data_t;
//	ret_func = other.ret_func;
//}


PITimer::~PITimer() {
	piMonitor.timers--;
	destroy();
}


void PITimer::initFirst() {
	setProperty("interval", 0.);
	lockRun = false;
	data_t = 0;
	ret_func = 0;
	imp = 0;
	init();
}


void PITimer::init() {
	destroy();
	switch (imp_mode) {
	case PITimer::Pool: imp = new _PITimerImp_Pool(); break;
	case PITimer::ThreadRT:
#ifdef PIP_TIMER_RT
		imp = new _PITimerImp_RT();
		break;
#else
		piCoutObj << "Warning: \"ThreadRT\" is not available at this system! Using \"Thread\".";
#endif
	case PITimer::Thread: imp = new _PITimerImp_Thread(); break;
	default: piCout << "Fatal: invalid implementation() of" << this << "!"; assert(0);
	}
	if (imp == 0) return;
	imp->tfunc = tickImpS;
	imp->parent = this;
}


void PITimer::destroy() {
	//piCout << "destroy" << this << imp;
	if (imp == 0) return;
	imp->stop(true); ///BUG: WTF FreeRTOS segfault on this!
	delete imp;
	imp = 0;
}


void PITimer::tickImp() {
	if (!isRunning()) return;
	if (lockRun) lock();
	if (ret_func != 0) ret_func(data_t, 1);
	tick(data_t, 1);
	tickEvent(data_t, 1);
	maybeCallQueuedEvents();
	piForeach (Delimiter & i, delims) {
		if (i.delim > ++(i.tick)) continue;
		i.tick = 0;
		if (i.slot != 0) i.slot(data_t, i.delim);
		else if (ret_func != 0) ret_func(data_t, i.delim);
		tick(data_t, i.delim);
		tickEvent(data_t, i.delim);
	}
	if (lockRun) unlock();
}


bool PITimer::waitForFinish(int timeout_msecs) {
	if (timeout_msecs < 0) {
		while (isRunning())
			msleep(PIP_MIN_MSLEEP);
		return true;
	}
	PITimeMeasurer tm;
	while (isRunning() && tm.elapsed_m() < timeout_msecs)
		msleep(PIP_MIN_MSLEEP);
	return tm.elapsed_m() < timeout_msecs;
}