/*
 * exec_cmds.c
 *
 * Copyright (c) 2009 Ismael Gomez-Miguelez, UPC <ismael.gomez at tsc.upc.edu>. All rights reserved.
 *
 *
 * This file is part of ALOE.
 *
 * ALOE 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.
 *
 * ALOE 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 ALOE.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/* standard libraries */
#include <stdlib.h>
#include <string.h>
#include "phal_hw_api.h"
#include "phid.h"
#include "phal_daemons.h"

#include "pkt.h"
#include "set.h"
#include "daemon.h"

#include "set.h"
#include "str.h"
#include "rtcobj.h"
#include "phobj.h"
#include "app.h"

/** Enable this constant to print debugging messages
 */

#include "exec.h"

#include "exec_cmds.h"
#include "swman_cmds.h"
#include "phitf.h"

/** wait timeslots before printing rtfault again
 */
#define RTPRINT_GUARD_TS    20

#define SYNC_MARGIN 4

#define REL_RTFAULT
/*#define KILL_RTCFAULT

#define PRINT_RTCFAULT
*/
#define NEW_STATUS_TOUT	20

/** maximum objects to control
 */
#define MAX_OBJECTS		100

struct hwapi_proc_i hwapi_obj[MAX_OBJECTS];

struct st_db {
	int max_ts;
	char name[16];
};
struct st_db st_db[PHAL_STATUS_MAX];

void init_state_str();
int update_report_table();

#define REPORT_INTERVAL	100
#define REPORT_WINDOW	100
#define REPORT_PROCS	MAX_OBJECTS

#define MAX_REPORT_ITF  5

struct report_data_itf {
	int usage;
};

struct report_data {
	int cpu_usec;
	int period;
	int sys_end;
	int tstamp;
	char executed;
};

struct report_obj {
	phobj_o obj;
	int hwapi_idx;
	int report_idx;
	int last_tstamp;
	int nof_itfs;
	hwitf_t itf_id[MAX_REPORT_ITF];
	phitf_o itf[MAX_REPORT_ITF];
	struct report_data report[REPORT_WINDOW];
};

static struct report_obj reports[REPORT_PROCS];

Set_o report_apps = NULL;

static daemon_o daemon;
static pkt_o pkt;

int lastprint;

#define NOF_OBJ	100
#define NOF_ITF	100
#define NOF_STR	1000
#define NOF_VAR	500
#define NOF_APP 10
#define NOF_SET	1000
#define NOF_SETMEM 1000


void send_rtfault(int tstamp, time_t *tdata, struct hwapi_proc_i *proc, float C);


void exec_alloc() {
#ifdef MTRACE
	mtrace();
#endif
	hwapi_mem_prealloc(NOF_OBJ, phobj_sizeof);
	hwapi_mem_prealloc(NOF_OBJ, rtcobj_sizeof);
	hwapi_mem_prealloc(NOF_ITF, phitf_sizeof);
	hwapi_mem_prealloc(NOF_STR, str_sizeof);
	hwapi_mem_prealloc(NOF_APP, app_sizeof);
	hwapi_mem_prealloc(NOF_SET, set_sizeof);
	hwapi_mem_prealloc(NOF_SETMEM, setmember_sizeof);
	hwapi_mem_prealloc(1, daemon_sizeof);
	hwapi_mem_prealloc(1, pkt_sizeof);
}

void exec_init(daemon_o d) {
	report_apps = Set_new(0, NULL);
	assert(report_apps);
	daemon = d;
	pkt = daemon_pkt(daemon);
	init_state_str();
	lastprint = 0;
}

int imean(int *x, int elem_sz, int num_elems) {
	int i;
	int res = 0;
	char *ptr = (char*) x;

	for (i = 0; i < num_elems; i++) {
		res += *((int*) ptr);
		ptr += elem_sz;
	}

	return (int) res / num_elems;
}

float ivar(int mean, int *x, int elem_sz, int num_elems) {
	int i;
	float res = 0;
	char *ptr = (char*) x;

	for (i = 0; i < num_elems; i++) {
		res += *((int*) ptr) * *((int*) ptr);
		ptr += elem_sz;
	}

	return sqrt(res / num_elems - mean * mean);
}

int imax(int *x, int elem_sz, int num_elems) {
	int i;
	int max = -99999999;
	char *ptr = (char*) x;

	for (i = 0; i < num_elems; i++) {
		if (*((int*) ptr) > max) {
			max = *((int*) ptr);
		}
		ptr += elem_sz;
	}

	return max;
}

void save_exec_stats(struct hwapi_proc_i *procs, int nof_procs) {
	int tstamp;
	int i, j, k;
	app_o app;
	struct hwapi_cpu_i cpu;

	hwapi_hwinfo_cpu(&cpu);

	tstamp = get_tstamp();

	for (i = 0; i < REPORT_PROCS; i++) {
		if (reports[i].obj) {
			k = reports[i].hwapi_idx;
			if (procs[k].obj_id != reports[i].obj->obj_id) {
#ifdef DEB
				daemon_info(daemon, "Caution, object 0x%x not reporting", reports[i].obj->obj_id);
#endif
				update_report_table();
				break;
			} else {
				if (tstamp >= reports[i].last_tstamp + 1) {
					reports[i].report[reports[i].report_idx].cpu_usec
							= procs[k].sys_cpu;
					reports[i].report[reports[i].report_idx].period
							= procs[k].sys_period;
					reports[i].report[reports[i].report_idx].sys_end
							= procs[k].sys_end;
					reports[i].report[reports[i].report_idx].executed = 1;
					reports[i].report[reports[i].report_idx].tstamp
							= procs[k].obj_tstamp;
					reports[i].last_tstamp = tstamp;
					reports[i].obj->rtc->tstamp = procs[k].obj_tstamp;
					reports[i].report_idx++;
				}
				if (reports[i].report_idx == REPORT_WINDOW) {
					reports[i].report_idx = 0;
					for (j = 0; j < reports[i].nof_itfs; j++) {
						reports[i].itf[j]->fifo_usage = hwapi_itf_status_id(
								reports[i].itf_id[j]);
						if (reports[i].itf[j]->fifo_usage < 0) {
#ifdef DEB
							daemon_info(daemon, "Caution, object 0x%x not reporting (itf failed)", reports[i].obj->obj_id);
#endif
							update_report_table();
							break;
						}
					}
					reports[i].obj->rtc->nvcs = procs[k].sys_nvcs;
					reports[i].obj->rtc->cpu_usec = imean(
							&reports[i].report[0].cpu_usec,
							sizeof(struct report_data), REPORT_WINDOW);
					reports[i].obj->rtc->mean_period = imean(
							&reports[i].report[0].period,
							sizeof(struct report_data), REPORT_WINDOW);
					reports[i].obj->rtc->max_usec = imax(
							&reports[i].report[0].cpu_usec,
							sizeof(struct report_data), REPORT_WINDOW);
					reports[i].obj->rtc->var_cpu = ivar(
							reports[i].obj->rtc->cpu_usec,
							&reports[i].report[0].cpu_usec,
							sizeof(struct report_data), REPORT_WINDOW);
					reports[i].obj->rtc->mean_mac
							= (int) reports[i].obj->rtc->cpu_usec * cpu.C;
					reports[i].obj->rtc->max_mac
							= (int) reports[i].obj->rtc->max_usec * cpu.C;
					reports[i].obj->rtc->start_usec = imax(
							&reports[i].report[0].sys_end,
							sizeof(struct report_data), REPORT_WINDOW);
					reports[i].obj->rtc->end_usec = procs[k].sys_end;
					reports[i].obj->core_idx = procs[k].core_idx;
				}
			}
		}
	}

	if (!report_apps)
		return;

	pkt_clear(pkt);
	for (i = 0; i < Set_length(report_apps); i++) {
		app = Set_get(report_apps, i);
		assert(app);
		if (tstamp - app->next_pending_tstamp > REPORT_INTERVAL) {
			pkt_put(pkt, FIELD_APP, app, app_topkt);
			daemon_sendto(daemon, SWMAN_APPINFOREPORT, 0, DAEMON_SWMAN);
			app->next_pending_tstamp = tstamp;
			return;
		}
	}
}

int update_report_table() {
	int i, j, k, l, n, m, p;
	phobj_o obj;
	phitf_o itf;
	app_o app;

	memset(reports, 0, sizeof(struct report_obj) * REPORT_PROCS);

	n = hwapi_proc_list(hwapi_obj, MAX_OBJECTS);
	if (!n)
		return 0;

	assert(n > 0);

	l = 0;
	for (k = 0; k < Set_length(report_apps); k++) {
		app = Set_get(report_apps, k);
		assert(app);

		for (i = 0; i < Set_length(app->objects); i++) {
			obj = Set_get(app->objects, i);
			assert(obj);
			j = 0;
			while (j < n && hwapi_obj[j].obj_id != obj->obj_id)
				j++;
			if (j < n) {
				reports[l].obj = obj;
				reports[l].hwapi_idx = j;
				p = 0;
				for (m = 0; m < Set_length(obj->itfs) && m < MAX_REPORT_ITF; m++) {
					itf = Set_get(obj->itfs, m);
					assert(itf);
					if (itf->mode & FLOW_WRITE_ONLY || itf->xitf_id != 0) {
						reports[l].itf[p] = itf;
						reports[l].itf_id[p] = hwapi_itf_find(str_str(
								obj->objname), str_str(itf->name), itf->mode);
						p++;
					}
				}
				reports[l].nof_itfs = p;
				l++;
			}
		}
	}
	return 1;
}

int remove_report_idx(int idx) {
	int j;

	for (j = 0; j < REPORT_PROCS; j++) {
		if (reports[j].hwapi_idx > reports[idx].hwapi_idx) {
			reports[j].hwapi_idx--;
		}
	}
	memset(&reports[idx], 0, sizeof(struct report_obj));
	return 1;
}

/** Starts or stops reporting of objects execution statistics. 
 * 'app' here does not refer to a waveform but to a group of objects. This group will be used
 * for reporting and for starting/stoppping. So the field app_name in the entity app does not need 
 * to refer to a real waveform name, can be anyone, but unique.
 */
int exec_incmd_reportcmd(cmd_t *cmd) {
	int start;
	app_o app_pkt, app_local;

	pkt = daemon_pkt(daemon);

	start = pkt_getcmd(pkt) == EXEC_REPORTSTART;

	app_pkt = pkt_get(pkt, FIELD_APP, app_newfrompkt);
	assert(app_pkt);

	if (start) {
		app_local = Set_find(report_apps, app_pkt->name, app_findname);
		if (app_local) {
			daemon_error(daemon, "Group %s already reporting", str_str(
					app_pkt->name));
			app_delete(&app_pkt);
			return 0;
		}
		Set_put(report_apps, app_pkt);
#ifdef DEB
		daemon_info(daemon, "Added group %s to reports", str_str(app_pkt->name));
#endif
		if (!update_report_table()) {
			daemon_info(daemon, "Error in table.");
			return 0;
		}
	} else {
#ifdef DEB
		daemon_info(daemon, "Stopping report for app %s", str_str(app_pkt->name));
#endif
		app_local = Set_find(report_apps, app_pkt->name, app_findname);
		if (!app_local) {
			daemon_error(daemon, "Unknown object group %s", str_str(
					app_pkt->name));
			app_delete(&app_pkt);
			return 0;
		}
		Set_remove(report_apps, app_local);
		app_delete(&app_local);
		app_delete(&app_pkt);
		update_report_table();
	}

	return 1;
}

/** Processing Function
 *
 * REQUIRED PACKET FIELDS:
 *	- FIELD_OBJID: Id of the object to change the status.
 *	- FIELD_STATUS: New status for the object.
 *
 * ACTIONS:
 *	- Uses hw_api functions to set new status.
 *	- The previous function will return -1 if can't change stauts of the object because
 * either it does not exists or it is already in a change procedure.
 *
 * OUTPUT:
 *	- SWMAN_STATUSOK command if successfull
 *	- SWMAN_STATUSERR command if error
 */
int exec_incmd_setstatus(cmd_t *cmd) {
	status_t status;
	objid_t obj_id;
	int tstamp;
	struct hwapi_proc_i proc;

	status = (status_t) pkt_getvalue(pkt, FIELD_STATUS);
	tstamp = (int) pkt_getvalue(pkt, FIELD_TSTAMP);
	obj_id = (objid_t) pkt_getvalue(pkt, FIELD_OBJID);

	/* if received abnormal stop */
	if (status == PHAL_STATUS_ABNSTOP) {
		tstamp = get_tstamp() - 1;
#ifdef DEB
		daemon_info(daemon, "Abnormal STOP for object 0x%x", obj_id);
#endif
	}

	if (hwapi_proc_status_new(obj_id, status, tstamp) <= 0) {
		if (status != PHAL_STATUS_ABNSTOP) {
			daemon_error(daemon,
					"Can't change status for object 0x%x. Killing process...",
					obj_id);
		}
		if (!hwapi_proc_info(obj_id, &proc)) {
#ifdef DEB
			daemon_error(daemon, "Unknown object 0x%x", obj_id);
#endif
			return 0;
		}
		exec_statusrep(&proc, 0);
		hwapi_proc_kill(obj_id);
	} else {

#ifdef DEB
		daemon_info(daemon, "Set new status %d for object %d at tstamp %d", status, obj_id, tstamp);
#endif
	}

	if (status == PHAL_STATUS_RUN) {
		hwapi_proc_tstamp_set(obj_id, tstamp);
	}

	/* *a successful change will be reported by exec_run() function when it detects it has been correctly
	 realized */
	return 1;
}

/** @defgroup sensors_exec EXEC Sensor Monitor Function
 *
 * EXEC Sensor, after processing all possible input commands, has the tasks to monitor
 * the object execution status and report changes or fault to manger (SW MANAGER).
 *
 * Status succesfull or error change report is executed by exec_statusrep function.
 *
 * @ingroup sensors
 *
 * @{
 */

/** Exec Status Report Function
 *
 * This function build a SWMAN_STATUSOK or SWMAN_STATUSERR commands and sends it. It indicates the successfull or
 * error in an status change procedure for an object
 * @todo appid
 *
 * @param obj Pointer to hwapi_proc_i structure of the object
 * @param ok 1 for STATUSOK 0 for STATUSERR
 *
 * @return 1 if send ok
 * @return -1 if send error
 */
int exec_statusrep(struct hwapi_proc_i *obj, int ok) {
	assert(daemon && obj);
	pkt_clear(pkt);

	pkt_putvalue(pkt, FIELD_APPID, (uint) obj->app_id);
	pkt_putvalue(pkt, FIELD_OBJID, (uint) obj->obj_id);
	pkt_putvalue(pkt, FIELD_STATUS, (uint) obj->status.cur_status);

	daemon_sendto(daemon, ok ? SWMAN_STATUSOK : SWMAN_STATUSERR, 0,
			DAEMON_SWMAN);

#ifdef DEB
	daemon_info(daemon, "Sent status %s %d for object %s", ok ? "ok" : "error", obj->status.cur_status,
			obj->obj_name);
#endif
	return 1;
}

int last_msg_ts = 0;

/** Exec Background Function
 *
 * This function implements the monitoring of the execution status of the objects.
 *
 * @todo Document this funciton
 * @todo Rewrite this function
 */
void exec_background(cmd_t *cmd) {
	int i, n;
	int tstamp;
	struct hwapi_cpu_i cpu;
	time_t tdata[3];

	hwapi_hwinfo_cpu(&cpu);

	get_time(&tdata[2]);
	tstamp = time_to_tstamp(&tdata[2]);

	memset(hwapi_obj, 0, MAX_OBJECTS * sizeof(struct hwapi_proc_i));
	n = hwapi_proc_list(hwapi_obj, MAX_OBJECTS);
	if (n < 0) {
		daemon_error(daemon, "Fatal error while getting object list");
		exit(-1);
	}

	save_exec_stats(hwapi_obj, n);

	for (i = 0; i < n; i++) {
		if (hwapi_obj[i].obj_id) {
			if (!hwapi_getsubslot_idx()) {
				/** if object died, report and remove it
				 */
				if (!cpu.debug_level && !hwapi_obj[i].pid
						&& hwapi_obj[i].status.cur_status) {
#ifdef DEB
					daemon_info(daemon, "Caution object %s died!", hwapi_obj[i].obj_name);
#endif
					if (!hwapi_proc_remove(hwapi_obj[i].obj_id)) {
						daemon_error(daemon, "Error removing process 0x%x",
								hwapi_obj[i].obj_id);
					}
					if (hwapi_obj[i].status.cur_status != PHAL_STATUS_ABNSTOP
							&& hwapi_obj[i].status.cur_status
									!= PHAL_STATUS_STOP) {
						exec_statusrep(&hwapi_obj[i], 0);
					}
				} else if (hwapi_obj[i].change_progress
						&& hwapi_obj[i].status.cur_status != PHAL_STATUS_STEP) {


					/** ack new status */
					if (hwapi_obj[i].status.cur_status
							== hwapi_obj[i].status.next_status
							&& hwapi_obj[i].change_progress == 2) {
						if (hwapi_proc_status_ack(hwapi_obj[i].obj_id) == 1) {
							exec_statusrep(&hwapi_obj[i], 1);
						} else {
							/* old data */
							break;
						}
					} else {
						if (tstamp > hwapi_obj[i].status.next_tstamp
								+ st_db[hwapi_obj[i].status.next_status].max_ts
								&& hwapi_obj[i].status.next_tstamp
								&& !cpu.debug_level) {


							daemon_info(
									daemon,
									"Caution object %s did not changed to %s after %d-%d=%d tslots (%d!=%d)",
									hwapi_obj[i].obj_name, st_db[hwapi_obj[i].status.next_status].name, tstamp,
									hwapi_obj[i].status.next_tstamp, tstamp
											- hwapi_obj[i].status.next_tstamp,
									hwapi_obj[i].status.cur_status,
									hwapi_obj[i].status.next_status);

							/* remove process */
							hwapi_proc_kill(hwapi_obj[i].obj_id);

							/** notify to manager object did not changed its status
							 */
							hwapi_obj[i].status.cur_status
									= PHAL_STATUS_ABNSTOP;
							exec_statusrep(&hwapi_obj[i], 0);
						}
					}
				}
				if (hwapi_obj[i].status.cur_status == PHAL_STATUS_RUN
						&& !cpu.debug_level && ((!hwapi_obj[i].relinquished
						&& hwapi_obj[i].cur_tstamp < tstamp)
						|| (hwapi_obj[i].relinquished
								&& hwapi_obj[i].cur_tstamp < tstamp - 1))
						&& tstamp > hwapi_last_sync() + SYNC_MARGIN
						&& hwapi_obj[i].relinquished) {

					memcpy(&tdata[1], &hwapi_obj[i].tdata[1], sizeof(time_t));
					get_time_interval(tdata);
					last_msg_ts = tstamp;

#ifdef PRINT_RTCFAULT
					daemon_info(
							daemon,
							"CAUTION REAL-TIME VIOLATION at %s. Object %s did not execute at ts %d (sync ts %d).\n\tObj TS=%d. Last Exec=%d:%d -> %d:%d Now=%d:%d\n",
							st_db[hwapi_obj[i].status.next_status].name, hwapi_obj[i].obj_name, tstamp,
							hwapi_last_sync(), hwapi_obj[i].cur_tstamp,
							hwapi_obj[i].tdata[1].tv_sec,
							hwapi_obj[i].tdata[1].tv_usec,
							hwapi_obj[i].tdata[2].tv_sec,
							hwapi_obj[i].tdata[2].tv_usec, tdata[2].tv_sec,
							tdata[2].tv_usec);
#endif
				}
			} else {
				if (hwapi_obj[i].status.cur_status == PHAL_STATUS_RUN
						&& !cpu.debug_level && !hwapi_obj[i].relinquished
						&& tstamp > hwapi_last_sync() + SYNC_MARGIN) {

					memcpy(&tdata[1], &hwapi_obj[i].tdata[1], sizeof(time_t));
					get_time_interval(tdata);
					if (tdata[0].tv_usec > hwapi_obj[i].usec_limit) {
						last_msg_ts = tstamp;
#ifdef PRINT_RTCFAULT

						daemon_info(
								daemon,
								"CAUTION REAL-TIME VIOLATION at %s. Object %s.\n\tTS=%d. Start=%d:%d Now=%d:%d ExecUS=%d ExecKOPTS=%d LimitUS=%d LimitKOPTS=%d\n",
								st_db[hwapi_obj[i].status.next_status].name, hwapi_obj[i].obj_name, tstamp,
								hwapi_obj[i].tdata[1].tv_sec, hwapi_obj[i].tdata[1].tv_usec,
								tdata[2].tv_sec, tdata[2].tv_usec,
								tdata[0].tv_usec, (int) (tdata[0].tv_usec*cpu.C),
								hwapi_obj[i].usec_limit, (int) (hwapi_obj[i].usec_limit*cpu.C));

#endif
						if (cpu.print_rtfaults) {
							send_rtfault(tstamp, tdata, &hwapi_obj[i],cpu.C);
						}
#ifdef KILL_RTCFAULT
						hwapi_proc_kill(hwapi_obj[i].obj_id);
#else
						hwapi_proc_relinquish(hwapi_obj[i].obj_id);
#endif

					}
				}
			}
		}
	}
}

void send_rtfault(int tstamp, time_t *tdata, struct hwapi_proc_i *proc, float C) {
	void *x;

	pkt_clear(pkt);
	pkt_putvalue(pkt, FIELD_TSTAMP, tstamp);
	x = pkt_putptr(pkt, FIELD_TIME, sizeof(time_t) * 3);
	if (x) {
		memcpy(x, tdata, sizeof(time_t) * 3);
	}
	x = pkt_putptr(pkt, FIELD_EXEINFO, sizeof(struct hwapi_proc_i));
	if (x) {
		memcpy(x, proc, sizeof(struct hwapi_proc_i));
	}

	pkt_putfloat(pkt, FIELD_C, C);
	daemon_sendto(daemon, SWMAN_RTFAULT, 0, DAEMON_SWMAN);
}

/** @} */

void init_state_str() {
	int i;

	/* configure status names */
	memset(st_db, 0, sizeof(struct st_db));

	for (i = 0; i < PHAL_STATUS_MAX; i++) {
		switch (i) {
		case PHAL_STATUS_STARTED:
			st_db[i].max_ts = LOAD_LIMIT_TS;
			sprintf(st_db[i].name, "LOAD");
			break;
		case PHAL_STATUS_INIT:
			st_db[i].max_ts = INIT_LIMIT_TS;
			sprintf(st_db[i].name, "INIT");
			break;
		case PHAL_STATUS_RUN:
			st_db[i].max_ts = RUN_LIMIT_TS;
			sprintf(st_db[i].name, "RUN");
			break;
		case PHAL_STATUS_PAUSE:
			st_db[i].max_ts = PAUSE_LIMIT_TS;
			sprintf(st_db[i].name, "PAUSE");
			break;
		case PHAL_STATUS_ABNSTOP:
		case PHAL_STATUS_STOP:
			st_db[i].max_ts = STOP_LIMIT_TS;
			sprintf(st_db[i].name, "STOP");
			break;
		case PHAL_STATUS_STEP:
			st_db[i].max_ts = STEP_LIMIT_TS;
			sprintf(st_db[i].name, "STEP");
			break;
		}
	}

}