/*
 * batch_manager.cpp
 *
 */


#include "batch_manager.h"

#include "bsp_batch.h"
#include "bsp_interrupts.h"

#include "rtlog.h"

#include "mrk.h"
#include "mrk_net.h"

#include "mex_hobbit_register.h"

#include "bsp_srio_mini_driver.h"

#include "batch_synch_ext.h"

volatile batch_synch_ext_recorded_t batch_synch_ext_recorded;

#include "rtg_high_resolution_timer.h"

//#define RTG_HRT_HAS_SYNCH

#ifdef RTG_HRT_HAS_SYNCH
#define rtg_hrt_synch_time_microseconds_ rtg_hrt_synch_time_microseconds
#else

//typedef unsigned long long rtg_hrt_microseconds_t;

static rtg_hrt_microseconds_t rtg_hrt_synch_time_microseconds_(unsigned long long us, signed long long* time_shift, signed long long* align_delay)
{
    return 0;
}

signed long long rtg_hrt_ticks_get_base()
{
    return 0;
}

rtg_hrt_ticks_t rtg_hrt_ticks_local_to_global(rtg_hrt_ticks_t t)
{
    return t+rtg_hrt_ticks_get_base();
}

rtg_hrt_ticks_t rtg_hrt_get_local(void)
{
    return rtg_hrt_get();
}

//rtg_hrt_microseconds_t rtg_hrt_to_microseconds_longlong(rtg_hrt_ticks_t ticks)
//{
//    return 0;
//}

#endif

static mrk_thread_t batch_thread;
static uint32_t batch_stack[4096];
static unsigned int batch_udp_port=BATCH_SYNCH_UDP_MASTER_PORT; //51110;
static mrk_net_udp_socket_t batch_socket;

static mrk_event_t batch_event;

#define batch_info_pkt_t batch_synch_ext_pkt_t

#define BATCH_ATTR_USED 	__attribute__((used))

struct batch_manager_cx_t
{
	enum { max_handlers = 4};

	unsigned int counter;
	volatile unsigned int pending;

	unsigned int ack;
	unsigned int overrun;

	volatile unsigned long long batch_raw_us;
	volatile unsigned long long batch_us;
	unsigned long batch_64us;
	int synch_period;
	unsigned int sync_counter;
	volatile int master;
	volatile int broadcast_enable;

	volatile unsigned int slave_id;
	volatile unsigned long slave_address;
	volatile unsigned long slave_event;

	volatile unsigned int sim_pacing;
	volatile unsigned int sim_period;

	unsigned long long last_synch;
	unsigned long long align;
	batch_info_pkt_t pkt;

	volatile unsigned int registered_handlers;
	BatchHandler* hList[max_handlers];

	unsigned int target_port;
	unsigned int target_ip;

	volatile unsigned int s_test;
	volatile unsigned int s_test_value;

	unsigned int tt_bn;

	unsigned int tt_degraded_synchs;

	volatile int slave_timetag;

	static void __monitor_invokable_raise() BATCH_ATTR_USED;
	static void __monitor_invokable_disable() BATCH_ATTR_USED;
	static void __monitor_invokable_enable() BATCH_ATTR_USED;
};

void batch_manager_cx_t::__monitor_invokable_raise()
{
	if (bsp_batch_raise())	//cannot simulate the batch...?
	{
		mrk_event_signal(&batch_event);
	}
}

void batch_manager_cx_t::__monitor_invokable_disable()
{
	bsp_batch_disable();	//cannot simulate the batch...
}

void batch_manager_cx_t::__monitor_invokable_enable()
{
	bsp_batch_enable();	//cannot simulate the batch...
}

static batch_manager_cx_t batch_manager_cx;


static void batch_handler(unsigned long long time, void*)
{
	++batch_manager_cx.counter;
	//if (batch_manager_cx.pending>10000)
	++batch_manager_cx.pending;
	if (batch_manager_cx.pending>1)
		++batch_manager_cx.overrun;

	batch_manager_cx.batch_us=time;
	//++batch_info_pkt.counter;
	//unsigned int history_index=batch_info_pkt.counter & 0x03;
	//batch_info_pkt.time=time>>6;
	//batch_info_pkt.time_history[history_index]=batch_info_pkt.time;
	mrk_event_signal(&batch_event);
}

#ifdef __MINGW32__
static void batch_socket_send(mrk_net_udp_socket_t*, const void* const buffer, unsigned short len, mrk_net_ip_raw_address_t destination, mrk_net_ip_port_t port, unsigned int timeout)
{
	static bool initialized;
	if (!initialized)
	{
		mrk_net_udp_socket(&batch_socket, "BATCH");
		mrk_net_udp_set_capacity(&batch_socket, 2);
		mrk_net_udp_bind(&batch_socket, mrk_net_myip_address(0), batch_udp_port);
		initialized=true;
	}
	mrk_net_udp_send(&batch_socket, buffer, len, destination, port, timeout);
}
#else
#define batch_socket_send mrk_net_udp_send
#endif

static void batch_exec(void)
{
	batch_manager_cx.target_port=BATCH_SYNCH_UDP_SLAVE_PORT; //batch_udp_port+1;
#ifdef __MINGW32__
	batch_manager_cx.target_ip=0;
#else
	batch_manager_cx.target_ip=BATCH_SYNCH_UPD_SLAVE_IP; //was 0; //mrk_net_myip_address_broadcast(0);
#endif

	//static unsigned int old_time=0;

	batch_manager_cx.slave_address=0x80FFFF00;
	batch_manager_cx.slave_id=0x33; //DSP0
	batch_manager_cx.slave_event=0xF;

	for(;;)
	{
		unsigned int batch_wait=batch_manager_cx.sim_period ? batch_manager_cx.sim_period : 100;

		mrk_sts_t s=mrk_event_wait(&batch_event, batch_wait);
		if (s==mrk_timeout)
		{
			if (batch_manager_cx.sim_period)
			{
				//batch_info_pkt.time+=mrk_ticks2ms( batch_manager_cx.sim_period)*(1000/64);
				//batch_manager_cx.batch_us=mrk_ticks2ms(batch_manager_cx.sim_period)*1000;
			    //int raised=bsp_batch_raise();
				BatchManager::fire();
			}

			if (batch_manager_cx.s_test)
			{
			    unsigned long long us=static_cast<unsigned long long>(batch_manager_cx.s_test_value);
				rtg_hrt_synch_time_microseconds_(us, 0, 0);
				batch_manager_cx.s_test=0;
			}
			continue;
		}


		{
			unsigned long long batch_raw_us=batch_manager_cx.batch_us;
			batch_manager_cx.batch_raw_us=batch_raw_us;
			signed long long t_base=rtg_hrt_to_microseconds_longlong(rtg_hrt_ticks_get_base());
			unsigned long long t_global=batch_manager_cx.batch_us;

			if (batch_manager_cx.slave_timetag)
			    t_global+=t_base;

			batch_manager_cx.batch_us=t_global;

			volatile unsigned long long t64us_big=batch_manager_cx.batch_us/64;	//volatile to prevent optimization
			//t64us_big=t64us_big/1000;
			unsigned int t64us=t64us_big;

			++batch_synch_ext_recorded.batch_intr;
			batch_synch_ext_recorded.batch_raw_time=batch_raw_us;
			batch_synch_ext_recorded.batch_tag=t64us;

			unsigned int period=t64us-batch_manager_cx.batch_64us;
			batch_manager_cx.batch_64us=t64us;

			++batch_manager_cx.pkt.counter;
			batch_manager_cx.pkt.counter_check=batch_manager_cx.pkt.counter;

			unsigned int history_index=batch_manager_cx.pkt.counter & 0x03;
			batch_manager_cx.pkt.time_us=batch_manager_cx.batch_us;
			batch_manager_cx.pkt.time_64us=t64us;
			batch_manager_cx.pkt.time_history[history_index]=batch_manager_cx.pkt.time_64us;

			if (batch_manager_cx.master)	//Set to DSP to then broadcast over Ethernet
			{
                rtlog_printf("BATCH", rtlog_trace, "Master: %u %u[*64us]", t64us, period);
				bsp_srio_mini_drv_memory_write_nowait(batch_manager_cx.slave_id, batch_manager_cx.slave_address, &batch_manager_cx.pkt, 256, batch_manager_cx.slave_event, false);
			}
			else
			    rtlog_printf("BATCH", rtlog_trace, "Start: %u %u[*64us]", t64us, period);
			/* 
			if(t-old_time<1)
			{
				rtlog_const_message("BATCH", rtlog_breaking_news|rtlog_error, "batch too short");
				continue;
			}
			*/
			//old_time=t;
			for(unsigned int i=0; i<batch_manager_cx.registered_handlers; ++i)
			{
			    if (batch_manager_cx.hList[i])
			        batch_manager_cx.hList[i]->batchExec(t64us); //batch_info_pkt.time);
			}

			if (batch_manager_cx.sim_pacing>0)
			{
				if (t64us>=batch_manager_cx.sim_pacing)
					/*mrk_net_udp_send*/batch_socket_send(&batch_socket, &batch_manager_cx.pkt, sizeof batch_manager_cx.pkt, batch_manager_cx.target_ip, batch_manager_cx.target_port, 0);
			}

		}

	}
}

class BatchManagerInitializer
{
public:
	BatchManagerInitializer()
    {
		bsp_batch_register_handler("batch", batch_handler, 0);
		bsp_batch_initialize();

		mex_hobbit_register_instance("batch", batch_manager_cx);

		batch_exec_init();

		//bsp_batch_enable();

		batch_manager_cx.broadcast_enable=1;
		batch_manager_cx.synch_period=500000; //500mseconds synchronize

		bsp_batch_enable();
    }

	static void batch_exec_init()
	{
		mrk_event_create(&batch_event, "BATCH");

#ifndef __MINGW32__
		mrk_net_udp_socket(&batch_socket, "BATCH");
		mrk_net_udp_set_capacity(&batch_socket, 2);
		mrk_net_udp_bind(&batch_socket, mrk_net_myip_address(0), batch_udp_port);
#endif

		batch_manager_cx.pkt.marker=0x43544142; //="BATC"
		batch_manager_cx.pkt.source=0;
		batch_manager_cx.pkt.intr_id=0x001;
		batch_manager_cx.pkt.end_marker=0x24144535;

		mrk_thread_create(&batch_thread, "BATCH", batch_exec, 0, 5, sizeof batch_stack, batch_stack, 0);
	}
};

void BatchManager::initialize()
{
	static BatchManagerInitializer theInitializer; //Take advantage of the compiler generated "singleton" initialization
/*
	bsp_batch_register_handler("batch", batch_handler, 0);
	bsp_batch_initialize();

	mex_hobbit_register_instance("batch", batch_manager_cx);

	batch_exec_init();

	bsp_batch_enable();

	batch_manager_cx.broadcast_enable=1;
*/
}

void BatchManager::setMaster(bool i_am_the_master)
{
    //batch_manager_cx.broadcast_enable=0;
	batch_manager_cx.master=i_am_the_master;
}

void BatchManager::setSlaveTimetag(bool i_am_a_slave)
{
	batch_manager_cx.slave_timetag=i_am_a_slave;
}

void BatchManager::batchEnd()
{
	++batch_manager_cx.ack;
	unsigned int tmp=bsp_interrupts_disable();
	//if (batch_manager_cx.pending>0)
	//	--batch_manager_cx.pending;

	batch_manager_cx.pending=0;

	bsp_interrupts_restore(tmp);

    rtlog_printf("BATCH", rtlog_trace, "End");
}

void BatchManager::registerHandler(BatchHandler& handler)
{
	if (batch_manager_cx.registered_handlers<batch_manager_cx_t::max_handlers)
	{
		unsigned int tmp=bsp_interrupts_disable();

		if (batch_manager_cx.registered_handlers<batch_manager_cx_t::max_handlers)
		{
			batch_manager_cx.hList[batch_manager_cx.registered_handlers]=&handler;
			++batch_manager_cx.registered_handlers;
		}
		bsp_interrupts_restore(tmp);
	}
}

void BatchManager::fire()
{
    if(bsp_batch_raise()!=0) //batch_manager_cx.__monitor_invokable_fire();
    {
    	batch_handler(rtg_hrt_to_microseconds(rtg_hrt_get_local()), 0);
		//mrk_event_signal(&batch_event);
    }
}

BatchHandler::~BatchHandler()
{
}

void BatchHandler::batchExec(unsigned int /*time_us*/)
{
}

#include "bsp_cache.h"

static void synch_send(const batch_info_pkt_t* pkt)
{
    rtlog_printf("BATCH", rtlog_info, "Broadcast: %u %uus", (unsigned int)pkt->counter, (unsigned int)pkt->time_us);
    if (batch_manager_cx.target_ip==0)
        batch_manager_cx.target_ip=mrk_net_myip_address_broadcast(0);
    /*mrk_net_udp_send*/batch_socket_send(&batch_socket, pkt, sizeof *pkt, batch_manager_cx.target_ip, batch_manager_cx.target_port, 0);
}

bool BatchManager::broadcastExternalSynch()
{
    static unsigned int log_filter;
    static bool first_received;

    if (!batch_manager_cx.broadcast_enable)
    {
        return false;
    }

	static unsigned int p_counter;

	batch_info_pkt_t* pkt=reinterpret_cast<batch_info_pkt_t*>(batch_manager_cx.slave_address);
	bsp_cache_data_invalidate(pkt, sizeof *pkt);
	if (pkt->counter==pkt->counter_check)
	{
	    if (!first_received)
	    {
	        p_counter=pkt->counter_check-1;
	    }
	    if ((!first_received) || (p_counter!=pkt->counter_check))
	    {
            first_received=true;
	        //batch_manager_cx.target_ip=mrk_net_myip_address_broadcast(0);
	        //mrk_net_udp_send(&batch_socket, pkt, sizeof *pkt, batch_manager_cx.target_ip, batch_manager_cx.target_port, 0);
	        batch_manager_cx.pkt.source=0;
	        p_counter=pkt->counter_check;
	        synch_send(pkt);
	        log_filter=0;
	        return true;
	    }
	}

	int log_level=rtlog_warning;
	if ((log_filter & (0x3FU))==1)
    //if ((log_filter & (~0x3FU))==0)
    {
        log_level=rtlog_warning|rtlog_breaking_news;
    }
    rtlog_printf("BATCH", log_level, "Broadcast: invalid: %u->%u?=%u (%u)", p_counter, (unsigned int)pkt->counter, (unsigned int)pkt->counter_check, log_filter);
    ++log_filter;
    return false;
}

bool BatchManager::broadcastExternalSynchFrom(const void* memory)
{
    if (!batch_manager_cx.broadcast_enable)
    {
        return false;
    }

	static unsigned int p_counter;

	static batch_info_pkt_t pkt_store;
	pkt_store=*reinterpret_cast<const batch_info_pkt_t*>(memory);
	batch_info_pkt_t* pkt=&pkt_store;
	if ((pkt->counter==pkt->counter_check) && (p_counter!=pkt->counter_check))
	{
	    //batch_manager_cx.target_ip=mrk_net_myip_address_broadcast(0);
		//mrk_net_udp_send(&batch_socket, pkt, sizeof *pkt, batch_manager_cx.target_ip, batch_manager_cx.target_port, 0);
	    batch_manager_cx.pkt.source=0;
	    synch_send(pkt);
	    p_counter=pkt->counter_check;
	    return true;
	}
    rtlog_printf("BATCH", rtlog_warning, "Broadcast: invalid packet from mem!");

    return false;
}

bool BatchManager::broadcastSynchDecode(uint64_t& time_us_, uint32_t& time_64us, const void* raw_pkt, unsigned int pkt_len)
{
#ifdef RTG_HRT_HAS_SYNCH
  	static unsigned long synch_received;

	unsigned long long time_us=0;


	++batch_manager_cx.sync_counter;

    const batch_info_pkt_t* pkt=reinterpret_cast<const batch_info_pkt_t*>(raw_pkt);
    if (pkt_len<sizeof *pkt)
        return false;
    if (pkt->marker!=0x43544142)
        return false;
    if (pkt->end_marker!=0x24144535)
        return false;
    if (pkt->counter_check!=pkt->counter)
        return false;

    time_us=pkt->time_us;
    time_64us=pkt->time_64us;

	unsigned long long delta=time_us-batch_manager_cx.last_synch;


    bool please_synch=false;

    if (batch_manager_cx.synch_period<0)
    {
        ++synch_received;
    	unsigned int s_period=-batch_manager_cx.synch_period;
    	if (synch_received>=s_period)
    	{
    		synch_received=0;
    		please_synch=true;
    	}
    }
    else if (batch_manager_cx.synch_period>0)
    {
    	unsigned int s_period=batch_manager_cx.synch_period;
    	synch_received+=delta;
    	if (synch_received>=s_period)
    	{
    		please_synch=true;
    		synch_received=0;
    	}
    }

	++batch_synch_ext_recorded.synch_count;
	batch_synch_ext_recorded.synch_tag=time_us/64;

    {
		if (please_synch)
		{
			signed long long time_shift;
			signed long long align_delay;
			unsigned long long now=rtg_hrt_synch_time_microseconds_(time_us, &time_shift, &align_delay);
			int d=delta;
			unsigned int t_us=pkt->time_us;
			signed int shift=time_shift; //assuming shift is always little enough to fit in 32bits
			unsigned int n=now/1000;

			++batch_synch_ext_recorded.align_count;
			batch_synch_ext_recorded.align_tag=time_us/64;
			batch_synch_ext_recorded.align_shift=time_shift/64;
			batch_synch_ext_recorded.align_delay=align_delay/64;

			rtlog_severity_code_t severity=rtlog_info;
			if ((shift>5000) || (shift<-5000))
				severity=rtlog_warning|rtlog_breaking_news;
			else if ((shift>64) || (shift<-64))
				severity=rtlog_warning; //|rtlog_breaking_news;
			if (batch_manager_cx.tt_bn)
				severity=severity|rtlog_breaking_news;
			rtlog_printf("TTSYNCH", severity, "[%u] %u (%d), shift=%d (%u) us", (unsigned int)pkt->counter, n, d, shift, t_us);
		}
    }

    batch_manager_cx.last_synch=time_us;
    time_us_=time_us;
    return true;
#else
    return false;
#endif
}


void BatchManager::degradedTimetagSync(unsigned long tt64us)
{
    rtlog_printf("TTSYNCH", rtlog_warning|rtlog_breaking_news, "Degraded tt synch %u", (unsigned int)tt64us);
    ++batch_manager_cx.tt_degraded_synchs;
    ++batch_manager_cx.pkt.counter;
    batch_manager_cx.pkt.source=1;
    batch_manager_cx.pkt.spare[0]=batch_manager_cx.tt_degraded_synchs;
    batch_manager_cx.pkt.counter_check=batch_manager_cx.pkt.counter;
    batch_manager_cx.pkt.time_64us=tt64us;
    batch_manager_cx.pkt.time_us=tt64us*64;
    synch_send(&batch_manager_cx.pkt);
}

void BatchManager::batchSimulation()
{
	batch_manager_cx.sim_period=2;
}
