static QuadTree QuadTree_add_internal(QuadTree q, real *coord, real weight, int id, int level){
int i, dim = q->dim, ii;
node_data nd = NULL;
int max_level = q->max_level;
int idd;
/* Make sure that coord is within bounding box */
for (i = 0; i < q->dim; i++) {
if (coord[i] < q->center[i] - q->width - 1.e5*MACHINEACC*q->width || coord[i] > q->center[i] + q->width + 1.e5*MACHINEACC*q->width) {
#ifdef DEBUG_PRINT
fprintf(stderr,"coordinate %f is outside of the box:{%f, %f}, \n(q->center[i] - q->width) - coord[i] =%g, coord[i]-(q->center[i] + q->width) = %g\n",coord[i], (q->center[i] - q->width), (q->center[i] + q->width),
(q->center[i] - q->width) - coord[i], coord[i]-(q->center[i] + q->width));
#endif
//return NULL;
}
}
if (q->n == 0){
/* if this node is empty right now */
q->n = 1;
q->total_weight = weight;
q->average = MALLOC(sizeof(real)*dim);
for (i = 0; i < q->dim; i++) q->average[i] = coord[i];
nd = node_data_new(q->dim, weight, coord, id);
assert(!(q->l));
q->l = SingleLinkedList_new(nd);
} else if (level < max_level){
/* otherwise open up into 2^dim quadtrees unless the level is too high */
q->total_weight += weight;
for (i = 0; i < q->dim; i++) q->average[i] = ((q->average[i])*q->n + coord[i])/(q->n + 1);
if (!q->qts){
q->qts = MALLOC(sizeof(QuadTree)*(1<<dim));
for (i = 0; i < 1<<dim; i++) q->qts[i] = NULL;
}/* done adding new quadtree, now add points to them */
/* insert the old node (if exist) and the current node into the appropriate child quadtree */
ii = QuadTree_get_quadrant(dim, q->center, coord);
assert(ii < 1<<dim && ii >= 0);
if (q->qts[ii] == NULL) q->qts[ii] = QuadTree_new_in_quadrant(q->dim, q->center, (q->width)/2, max_level, ii);
q->qts[ii] = QuadTree_add_internal(q->qts[ii], coord, weight, id, level + 1);
assert(q->qts[ii]);
if (q->l){
idd = node_data_get_id(SingleLinkedList_get_data(q->l));
assert(q->n == 1);
coord = node_data_get_coord(SingleLinkedList_get_data(q->l));
weight = node_data_get_weight(SingleLinkedList_get_data(q->l));
ii = QuadTree_get_quadrant(dim, q->center, coord);
assert(ii < 1<<dim && ii >= 0);
if (q->qts[ii] == NULL) q->qts[ii] = QuadTree_new_in_quadrant(q->dim, q->center, (q->width)/2, max_level, ii);
q->qts[ii] = QuadTree_add_internal(q->qts[ii], coord, weight, idd, level + 1);
assert(q->qts[ii]);
/* delete the old node data on parent */
SingleLinkedList_delete(q->l, node_data_delete);
q->l = NULL;
}
(q->n)++;
} else {
assert(!(q->qts));
/* level is too high, append data in the linked list */
(q->n)++;
q->total_weight += weight;
for (i = 0; i < q->dim; i++) q->average[i] = ((q->average[i])*q->n + coord[i])/(q->n + 1);
nd = node_data_new(q->dim, weight, coord, id);
assert(q->l);
q->l = SingleLinkedList_prepend(q->l, nd);
}
return q;
}
void *devman_svc_handler(void *args)
{
gw_msg_t *msg, *rsp_msg;
sn_data_t *tmp_sn_data;
node_data_t *tmp_node_data;
gw_t *gw = (gw_t *) args;
bool_t ret;
/* initialize infobase */
while ((msg = gw_msg_recv(devman_msgq)) != NULL)
{
if (msg->type == GW_MSG_SYSTEM_QUIT)
{
gw_msg_free(msg);
gw_log_print(GW_LOG_LEVEL_INFO, "DEV management module receive a quit msg!\n");
break;
}
if ((msg->srcsvc >= 0x1000) && (msg->srcsvc < 0x2000))
{
/* this msg is from south interface */
gw_debug_print("module dev management receive a msg type is %d\n", msg->type);
switch (msg->type)
{
case DEVICE_LOGIN_REQUEST:
{
tmp_node_data = NULL;
tmp_node_data = node_data_get(msg->devid, NUI_LEN);
if (tmp_node_data == NULL)
{
gw_log_print(GW_LOG_LEVEL_INFO, "Receive a new device!\n");
if((msg->data != NULL) && (msg->datasize == 4))
{
uint32_t snet_id = *((uint32_t *)msg->data);
tmp_sn_data = sn_data_get(snet_id);
if (tmp_sn_data == NULL)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "Sn data does not exist!\n");
gw_msg_free(msg);
}
else
{
tmp_node_data = node_data_new();
memcpy(tmp_node_data->nui, msg->devid, NUI_LEN);
tmp_node_data->pdata = tmp_sn_data;
tmp_node_data->state = STA_REG_TO_SN;
tmp_node_data->last_update_time = time(NULL);
node_data_add(tmp_sn_data, tmp_node_data);
devman_stats.login_request_num ++;
msg->srcsvc = devman_svc.id;
msg->dstsvc = PLAT_DM_SVC;
gw_msg_send(gw, msg);
}
}
else
{
gw_log_print(GW_LOG_LEVEL_WARNING, "No sn id included in reg msg, data size is %d, data is %d!\n", msg->datasize, msg->data);
gw_msg_free(msg);
}
}
else
{
// if (tmp_node_data->state == STA_REG_TO_PLAT)
// {
// /* already registered */
// rsp_msg = gen_ack_msg(msg, EC_SUCCESS);
// gw_msg_free(msg);
// gw_msg_send(gw, rsp_msg);
// gw_debug_print("node already registered!\n");
// }
// else
// {
msg->srcsvc = devman_svc.id;
msg->dstsvc = PLAT_DM_SVC;
gw_msg_send(gw, msg);
// }
}
break;
}
case DEVICE_LOGOUT_REQUEST:
{
tmp_node_data = NULL;
tmp_node_data = node_data_get(msg->devid, NUI_LEN);
if (tmp_node_data == NULL)
{
rsp_msg = gen_ack_msg(msg, EC_DEV_UNJOIN);
gw_debug_print("Leave msg receive error, node info not exist!\n");
gw_msg_free(msg);
gw_msg_send(gw, rsp_msg);
}
else
{
// if (tmp_node_data->state != STA_REG_TO_PLAT)
// {
// /* already registered */
// rsp_msg = gen_ack_msg(msg, EC_DEV_UNREG);
// gw_msg_free(msg);
// gw_msg_send(gw, rsp_msg);
// gw_debug_print("node not registered!\n");
// }
// else
// {
devman_stats.logout_request_num ++;
msg->srcsvc = devman_svc.id;
msg->dstsvc = PLAT_DM_SVC;
gw_msg_send(gw, msg);
// }
}
break;
}
case DEVICE_UPDATE_REQUEST:
{
tmp_node_data = NULL;
tmp_node_data = node_data_get(msg->devid, NUI_LEN);
// if ((tmp_node_data == NULL) || (tmp_node_data->state
// != STA_REG_TO_PLAT))
// {
// //rsp_msg = gen_ack_msg(msg, EC_DEV_UNREG);
// gw_debug_print("Dev info update error! not registered!\n");
// gw_msg_free(msg);
// //gw_msg_send(gw, rsp_msg);
// }
// else
// {
if (tmp_node_data == NULL)
{
//rsp_msg = gen_ack_msg(msg, EC_DEV_UNREG);
gw_debug_print("Dev info update error! not registered!\n");
gw_msg_free(msg);
//gw_msg_send(gw, rsp_msg);
}
else
{
devman_stats.info_update_num ++;
tmp_node_data->last_update_time = time(NULL);
msg->srcsvc = devman_svc.id;
msg->dstsvc = PLAT_DM_SVC;
gw_msg_send(gw, msg);
}
break;
}
case PASSIVE_CONFIG_RESPONSE:
{
gw_debug_print("Receive a passive config response packet!\n");
gw_msg_free(msg);
break;
}
default:
{
gw_log_print(GW_LOG_LEVEL_WARNING, "Dev management receive unknown msg type from sn!\n");
gw_msg_free(msg);
}
}
}
else if (msg->srcsvc >= 0x2000)
{
/* this msg is from north interface */
switch (msg->type)
{
case DEVICE_LOGIN_RESPONSE:
{
gw_debug_print("dev manage receive a login response!\n");
tmp_node_data = NULL;
tmp_node_data = node_data_get(msg->devid, NUI_LEN);
if (tmp_node_data == NULL)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "Node data does not exist!\n");
gw_msg_free(msg);
}
else
{
if (msg->retcode == 0)
{
/* node register success */
devman_stats.login_success_num ++;
if (tmp_node_data->state != STA_REG_TO_PLAT)
{
tmp_node_data->state = STA_REG_TO_PLAT;
tmp_node_data->pdata->reg_node_num++;
}
}
else
{
if (tmp_node_data->state == STA_REG_TO_PLAT)
{
tmp_node_data->state = STA_LOST_REG;
tmp_node_data->pdata->reg_node_num--;
}
}
msg->srcsvc = devman_svc.id;
msg->dstsvc = SN_DM_SVC;
ret = gw_msg_send(gw, msg);
if(ret == FALSE)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "msg send error!\n");
gw_msg_free(msg);
}
}
break;
}
case DEVICE_LOGOUT_RESPONSE:
{
gw_debug_print("receive a log out response message!\n");
tmp_node_data = NULL;
tmp_node_data = node_data_get(msg->devid, NUI_LEN);
if (tmp_node_data == NULL)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "Node data does not exist!\n");
gw_msg_free(msg);
}
else
{
if (tmp_node_data->state == STA_REG_TO_PLAT)
{
msg->srcsvc = devman_svc.id;
msg->dstsvc = SN_DM_SVC;
ret = gw_msg_send(gw, msg);
if(ret == FALSE)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "msg send error!\n");
gw_msg_free(msg);
}
if (msg->retcode == 0)
{
tmp_node_data->state = STA_LOST_REG;
}
}
else
{
gw_msg_free(msg);
gw_debug_print("Not registered yet!");
}
}
break;
}
case PASSIVE_CONFIG_REQUEST:
{
// gw_debug_print("dev manage passive config msg!\n");
tmp_node_data = NULL;
tmp_node_data = node_data_get(msg->devid, NUI_LEN);
if (tmp_node_data == NULL)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "Node data does not exist!\n");
gw_msg_free(msg);
}
else
{
if (tmp_node_data->state == STA_REG_TO_PLAT)
{
uint8_t *pt = (uint8_t *)msg->data;
uint32_t tmp_intv = *((uint32_t *)(pt+6))/1000;
// if(tmp_intv != tmp_node_data->update_interval)
// {
tmp_node_data->update_interval = tmp_intv;
gw_debug_print("device update interval is set to %d\n", tmp_intv);
msg->srcsvc = devman_svc.id;
msg->dstsvc = SN_DM_SVC;
ret = gw_msg_send(gw, msg);
if(ret == FALSE)
{
gw_log_print(GW_LOG_LEVEL_WARNING, "msg send error!\n");
gw_msg_free(msg);
}
// }else
// {
// gw_msg_free(msg);
// }
}
else
{
gw_msg_free(msg);
gw_debug_print("Not registered yet, can not config!");
}
}
break;
}
default:
gw_log_print(GW_LOG_LEVEL_WARNING, "unknown message type!\n");
gw_msg_free(msg);
break;
}
}
}
}
