/*************************************************************************
* This function performs an edge-based FM refinement
**************************************************************************/
int BalanceMyLink(CtrlType *ctrl, GraphType *graph, idxtype *home, int me,
int you, float *flows, float maxdiff, float *diff_cost, float *diff_lbavg,
float avgvwgt)
{
int h, i, ii, j, k;
int nvtxs, ncon;
int nqueues, minval, maxval, higain, vtx, edge, totalv;
int from, to, qnum, index, nchanges, cut, tmp;
int pass, nswaps, nmoves, multiplier;
idxtype *xadj, *vsize, *adjncy, *adjwgt, *where, *ed, *id;
idxtype *hval, *nvpq, *inq, *map, *rmap, *ptr, *myqueue, *changes;
float *nvwgt, lbvec[MAXNCON], pwgts[MAXNCON*2], tpwgts[MAXNCON*2], my_wgt[MAXNCON];
float newgain;
float lbavg, bestflow, mycost;
float ipc_factor, redist_factor, ftmp;
FPQueueType *queues;
int mype;
MPI_Comm_rank(MPI_COMM_WORLD, &mype);
nvtxs = graph->nvtxs;
ncon = graph->ncon;
xadj = graph->xadj;
nvwgt = graph->nvwgt;
vsize = graph->vsize;
adjncy = graph->adjncy;
adjwgt = graph->adjwgt;
where = graph->where;
ipc_factor = ctrl->ipc_factor;
redist_factor = ctrl->redist_factor;
hval = idxmalloc(nvtxs*7, "hval");
id = hval + nvtxs;
ed = hval + nvtxs*2;
map = hval + nvtxs*3;
rmap = hval + nvtxs*4;
myqueue = hval + nvtxs*5;
changes = hval + nvtxs*6;
sset(ncon*2, 0.0, pwgts);
for (h=0; h<ncon; h++) {
tpwgts[h] = -1.0 * flows[h];
tpwgts[ncon+h] = flows[h];
}
for (i=0; i<nvtxs; i++) {
if (where[i] == me) {
for (h=0; h<ncon; h++) {
tpwgts[h] += nvwgt[i*ncon+h];
pwgts[h] += nvwgt[i*ncon+h];
}
}
else {
ASSERTS(where[i] == you);
for (h=0; h<ncon; h++) {
tpwgts[ncon+h] += nvwgt[i*ncon+h];
pwgts[ncon+h] += nvwgt[i*ncon+h];
}
}
}
/* we don't want any tpwgts to be less than zero */
for (h=0; h<ncon; h++) {
if (tpwgts[h] < 0.0) {
tpwgts[ncon+h] += tpwgts[h];
tpwgts[h] = 0.0;
}
if (tpwgts[ncon+h] < 0.0) {
tpwgts[h] += tpwgts[ncon+h];
tpwgts[ncon+h] = 0.0;
}
}
/*******************************/
/* insert vertices into queues */
/*******************************/
minval = maxval = 0;
multiplier = 1;
for (i=0; i<ncon; i++) {
multiplier *= (i+1);
maxval += i*multiplier;
minval += (ncon-1-i)*multiplier;
}
nqueues = maxval-minval+1;
nvpq = idxsmalloc(nqueues, 0, "nvpq");
ptr = idxmalloc(nqueues+1, "ptr");
inq = idxmalloc(nqueues*2, "inq");
queues = (FPQueueType *)(GKmalloc(sizeof(FPQueueType)*nqueues*2, "queues"));
for (i=0; i<nvtxs; i++)
hval[i] = Mc_HashVwgts(ncon, nvwgt+i*ncon) - minval;
for (i=0; i<nvtxs; i++)
nvpq[hval[i]]++;
ptr[0] = 0;
for (i=0; i<nqueues; i++)
ptr[i+1] = ptr[i] + nvpq[i];
for (i=0; i<nvtxs; i++) {
map[i] = ptr[hval[i]];
rmap[ptr[hval[i]]++] = i;
}
for (i=nqueues-1; i>0; i--)
ptr[i] = ptr[i-1];
ptr[0] = 0;
/* initialize queues */
for (i=0; i<nqueues; i++)
if (nvpq[i] > 0) {
FPQueueInit(queues+i, nvpq[i]);
FPQueueInit(queues+i+nqueues, nvpq[i]);
}
/* compute internal/external degrees */
idxset(nvtxs, 0, id);
idxset(nvtxs, 0, ed);
for (j=0; j<nvtxs; j++)
for (k=xadj[j]; k<xadj[j+1]; k++)
if (where[adjncy[k]] == where[j])
id[j] += adjwgt[k];
else
ed[j] += adjwgt[k];
nswaps = 0;
for (pass=0; pass<N_MOC_BAL_PASSES; pass++) {
idxset(nvtxs, -1, myqueue);
idxset(nqueues*2, 0, inq);
/* insert vertices into correct queues */
for (j=0; j<nvtxs; j++) {
index = (where[j] == me) ? 0 : nqueues;
newgain = ipc_factor*(float)(ed[j]-id[j]);
if (home[j] == me || home[j] == you) {
if (where[j] == home[j])
newgain -= redist_factor*(float)vsize[j];
else
newgain += redist_factor*(float)vsize[j];
}
FPQueueInsert(queues+hval[j]+index, map[j]-ptr[hval[j]], newgain);
myqueue[j] = (where[j] == me) ? 0 : 1;
inq[hval[j]+index]++;
}
/* bestflow = sfavg(ncon, flows); */
for (j=0, h=0; h<ncon; h++)
if (fabs(flows[h]) > fabs(flows[j])) j = h;
bestflow = fabs(flows[j]);
nchanges = nmoves = 0;
for (ii=0; ii<nvtxs/2; ii++) {
from = -1;
Mc_DynamicSelectQueue(nqueues, ncon, me, you, inq, flows, &from,
&qnum, minval, avgvwgt, maxdiff);
/* can't find a vertex in one subdomain, try the other */
if (from != -1 && qnum == -1) {
from = (from == me) ? you : me;
if (from == me) {
for (j=0; j<ncon; j++)
if (flows[j] > avgvwgt)
break;
}
else {
for (j=0; j<ncon; j++)
if (flows[j] < -1.0*avgvwgt)
break;
}
if (j != ncon)
Mc_DynamicSelectQueue(nqueues, ncon, me, you, inq, flows, &from,
&qnum, minval, avgvwgt, maxdiff);
}
if (qnum == -1)
break;
to = (from == me) ? you : me;
index = (from == me) ? 0 : nqueues;
higain = FPQueueGetMax(queues+qnum+index);
inq[qnum+index]--;
ASSERTS(higain != -1);
/*****************/
/* make the swap */
/*****************/
vtx = rmap[higain+ptr[qnum]];
myqueue[vtx] = -1;
where[vtx] = to;
nswaps++;
nmoves++;
/* update the flows */
for (j=0; j<ncon; j++)
flows[j] += (to == me) ? nvwgt[vtx*ncon+j] : -1.0*nvwgt[vtx*ncon+j];
/* ftmp = sfavg(ncon, flows); */
for (j=0, h=0; h<ncon; h++)
if (fabs(flows[h]) > fabs(flows[j])) j = h;
ftmp = fabs(flows[j]);
if (ftmp < bestflow) {
bestflow = ftmp;
nchanges = 0;
}
else {
changes[nchanges++] = vtx;
}
SWAP(id[vtx], ed[vtx], tmp);
for (j=xadj[vtx]; j<xadj[vtx+1]; j++) {
edge = adjncy[j];
tmp = (to == where[edge] ? adjwgt[j] : -adjwgt[j]);
INC_DEC(id[edge], ed[edge], tmp);
if (myqueue[edge] != -1) {
newgain = ipc_factor*(float)(ed[edge]-id[edge]);
if (home[edge] == me || home[edge] == you) {
if (where[edge] == home[edge])
newgain -= redist_factor*(float)vsize[edge];
else
newgain += redist_factor*(float)vsize[edge];
}
FPQueueUpdate(queues+hval[edge]+(nqueues*myqueue[edge]),
map[edge]-ptr[hval[edge]], newgain);
}
}
}
/****************************/
/* now go back to best flow */
/****************************/
nswaps -= nchanges;
nmoves -= nchanges;
for (i=0; i<nchanges; i++) {
vtx = changes[i];
from = where[vtx];
where[vtx] = to = (from == me) ? you : me;
SWAP(id[vtx], ed[vtx], tmp);
for (j=xadj[vtx]; j<xadj[vtx+1]; j++) {
edge = adjncy[j];
tmp = (to == where[edge] ? adjwgt[j] : -adjwgt[j]);
INC_DEC(id[edge], ed[edge], tmp);
}
}
for (i=0; i<nqueues; i++) {
if (nvpq[i] > 0) {
FPQueueReset(queues+i);
FPQueueReset(queues+i+nqueues);
}
}
if (nmoves == 0)
break;
}
/***************************/
/* compute 2-way imbalance */
/***************************/
sset(ncon, 0.0, my_wgt);
for (i=0; i<nvtxs; i++)
if (where[i] == me)
for (h=0; h<ncon; h++)
my_wgt[h] += nvwgt[i*ncon+h];
for (i=0; i<ncon; i++) {
ftmp = (pwgts[i]+pwgts[ncon+i])/2.0;
if (ftmp != 0.0)
lbvec[i] = fabs(my_wgt[i]-tpwgts[i]) / ftmp;
else
lbvec[i] = 0.0;
}
lbavg = savg(ncon, lbvec);
*diff_lbavg = lbavg;
/****************/
/* compute cost */
/****************/
cut = totalv = 0;
for (i=0; i<nvtxs; i++) {
if (where[i] != home[i])
totalv += vsize[i];
for (j=xadj[i]; j<xadj[i+1]; j++)
if (where[adjncy[j]] != where[i])
cut += adjwgt[j];
}
cut /= 2;
mycost = cut*ipc_factor + totalv*redist_factor;
*diff_cost = mycost;
/* free memory */
for (i=0; i<nqueues; i++)
if (nvpq[i] > 0) {
FPQueueFree(queues+i);
FPQueueFree(queues+i+nqueues);
}
GKfree((void **)&hval, (void **)&nvpq, (void **)&ptr, (void **)&inq, (void **)&queues, LTERM);
return nswaps;
}
void mavlink_command_long_msg_send(const struct command_long_msg* n_var0,
uint8_t* n_var1, uint8_t n_var2[80U])
{
uint8_t n_local0[33U] = {};
uint8_t* n_ref1 = n_local0;
float n_deref2 = n_var0->param1;
mavlink_pack_float((uint8_t*) n_ref1, 0U, n_deref2);
float n_deref3 = n_var0->param2;
mavlink_pack_float((uint8_t*) n_ref1, 4U, n_deref3);
float n_deref4 = n_var0->param3;
mavlink_pack_float((uint8_t*) n_ref1, 8U, n_deref4);
float n_deref5 = n_var0->param4;
mavlink_pack_float((uint8_t*) n_ref1, 12U, n_deref5);
float n_deref6 = n_var0->param5;
mavlink_pack_float((uint8_t*) n_ref1, 16U, n_deref6);
float n_deref7 = n_var0->param6;
mavlink_pack_float((uint8_t*) n_ref1, 20U, n_deref7);
float n_deref8 = n_var0->param7;
mavlink_pack_float((uint8_t*) n_ref1, 24U, n_deref8);
uint16_t n_deref9 = n_var0->command;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 28U, n_deref9);
uint8_t n_deref10 = n_var0->target_system;
mavlink_pack_uint8_t((uint8_t*) n_ref1, 30U, n_deref10);
uint8_t n_deref11 = n_var0->target_component;
mavlink_pack_uint8_t((uint8_t*) n_ref1, 31U, n_deref11);
uint8_t n_deref12 = n_var0->confirmation;
mavlink_pack_uint8_t((uint8_t*) n_ref1, 32U, n_deref12);
for (int32_t n_ix13 = 0; n_ix13 <= 32; n_ix13++) {
ASSERTS(n_ix13 > 0 && 2147483647 - n_ix13 >= 6 || n_ix13 <= 0);
if (n_ix13 + 6 >= 80) { } else {
uint8_t n_deref14 = n_ref1[n_ix13];
ASSERTS(n_ix13 > 0 && 2147483641 >= n_ix13 || n_ix13 <= 0);
ASSERTS(0 <= 6 + n_ix13 && 6 + n_ix13 < 80);
*&n_var2[(6 + n_ix13) % 80] = n_deref14;
}
}
mavlinkSendWithWriter(76U, 152U, 33U, n_var1, n_var2);
for (int32_t n_ix15 = 0; n_ix15 <= 38; n_ix15++) {
ASSERTS(n_ix15 > 0 && 2147483647 - n_ix15 >= 41 || n_ix15 <= 0);
ASSERTS(0 <= n_ix15 + 41 && n_ix15 + 41 < 80);
*&n_var2[(n_ix15 + 41) % 80] = 0U;
}
return;
}
void mavlink_sys_status_msg_send(const struct sys_status_msg* n_var0,
uint8_t* n_var1, uint8_t n_var2[80U])
{
uint8_t n_local0[31U] = {};
uint8_t* n_ref1 = n_local0;
uint32_t n_deref2 = n_var0->onboard_control_sensors_present;
mavlink_pack_uint32_t((uint8_t*) n_ref1, 0U, n_deref2);
uint32_t n_deref3 = n_var0->onboard_control_sensors_enabled;
mavlink_pack_uint32_t((uint8_t*) n_ref1, 4U, n_deref3);
uint32_t n_deref4 = n_var0->onboard_control_sensors_health;
mavlink_pack_uint32_t((uint8_t*) n_ref1, 8U, n_deref4);
uint16_t n_deref5 = n_var0->load;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 12U, n_deref5);
uint16_t n_deref6 = n_var0->voltage_battery;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 14U, n_deref6);
int16_t n_deref7 = n_var0->current_battery;
mavlink_pack_int16_t((uint8_t*) n_ref1, 16U, n_deref7);
uint16_t n_deref8 = n_var0->drop_rate_comm;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 18U, n_deref8);
uint16_t n_deref9 = n_var0->errors_comm;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 20U, n_deref9);
uint16_t n_deref10 = n_var0->errors_count1;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 22U, n_deref10);
uint16_t n_deref11 = n_var0->errors_count2;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 24U, n_deref11);
uint16_t n_deref12 = n_var0->errors_count3;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 26U, n_deref12);
uint16_t n_deref13 = n_var0->errors_count4;
mavlink_pack_uint16_t((uint8_t*) n_ref1, 28U, n_deref13);
int8_t n_deref14 = n_var0->battery_remaining;
mavlink_pack_int8_t((uint8_t*) n_ref1, 30U, n_deref14);
for (int32_t n_ix15 = 0; n_ix15 <= 30; n_ix15++) {
ASSERTS(n_ix15 > 0 && 2147483647 - n_ix15 >= 6 || n_ix15 <= 0);
if (n_ix15 + 6 >= 80) { } else {
uint8_t n_deref16 = n_ref1[n_ix15];
ASSERTS(n_ix15 > 0 && 2147483641 >= n_ix15 || n_ix15 <= 0);
ASSERTS(0 <= 6 + n_ix15 && 6 + n_ix15 < 80);
*&n_var2[(6 + n_ix15) % 80] = n_deref16;
}
}
mavlinkSendWithWriter(1U, 124U, 31U, n_var1, n_var2);
for (int32_t n_ix17 = 0; n_ix17 <= 40; n_ix17++) {
ASSERTS(n_ix17 > 0 && 2147483647 - n_ix17 >= 39 || n_ix17 <= 0);
ASSERTS(0 <= n_ix17 + 39 && n_ix17 + 39 < 80);
*&n_var2[(n_ix17 + 39) % 80] = 0U;
}
return;
}
void mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_msg_send(const
struct set_quad_swarm_led_roll_pitch_yaw_thrust_msg* n_var0,
uint8_t* n_var1,
uint8_t n_var2[80U])
{
uint8_t n_local0[46U] = {};
uint8_t* n_ref1 = n_local0;
uint8_t n_deref2 = n_var0->group;
mavlink_pack_uint8_t((uint8_t*) n_ref1, 32U, n_deref2);
uint8_t n_deref3 = n_var0->mode;
mavlink_pack_uint8_t((uint8_t*) n_ref1, 33U, n_deref3);
const int16_t* n_let4 = n_var0->roll;
for (int32_t n_ix5 = 0; n_ix5 <= 3; n_ix5++) {
int16_t n_deref6 = n_let4[n_ix5];
mavlink_pack_int16_t((uint8_t*) n_ref1, 0U + (uint8_t) n_ix5, n_deref6);
}
const int16_t* n_let7 = n_var0->pitch;
for (int32_t n_ix8 = 0; n_ix8 <= 3; n_ix8++) {
int16_t n_deref9 = n_let7[n_ix8];
mavlink_pack_int16_t((uint8_t*) n_ref1, 8U + (uint8_t) n_ix8, n_deref9);
}
const int16_t* n_let10 = n_var0->yaw;
for (int32_t n_ix11 = 0; n_ix11 <= 3; n_ix11++) {
int16_t n_deref12 = n_let10[n_ix11];
mavlink_pack_int16_t((uint8_t*) n_ref1, 16U + (uint8_t) n_ix11,
n_deref12);
}
const uint16_t* n_let13 = n_var0->thrust;
for (int32_t n_ix14 = 0; n_ix14 <= 3; n_ix14++) {
uint16_t n_deref15 = n_let13[n_ix14];
mavlink_pack_uint16_t((uint8_t*) n_ref1, 24U + (uint8_t) n_ix14,
n_deref15);
}
const uint8_t* n_let16 = n_var0->led_red;
for (int32_t n_ix17 = 0; n_ix17 <= 3; n_ix17++) {
uint8_t n_deref18 = n_let16[n_ix17];
mavlink_pack_uint8_t((uint8_t*) n_ref1, 34U + (uint8_t) n_ix17,
n_deref18);
}
const uint8_t* n_let19 = n_var0->led_blue;
for (int32_t n_ix20 = 0; n_ix20 <= 3; n_ix20++) {
uint8_t n_deref21 = n_let19[n_ix20];
mavlink_pack_uint8_t((uint8_t*) n_ref1, 38U + (uint8_t) n_ix20,
n_deref21);
}
const uint8_t* n_let22 = n_var0->led_green;
for (int32_t n_ix23 = 0; n_ix23 <= 3; n_ix23++) {
uint8_t n_deref24 = n_let22[n_ix23];
mavlink_pack_uint8_t((uint8_t*) n_ref1, 42U + (uint8_t) n_ix23,
n_deref24);
}
for (int32_t n_ix25 = 0; n_ix25 <= 45; n_ix25++) {
ASSERTS(n_ix25 > 0 && 2147483647 - n_ix25 >= 6 || n_ix25 <= 0);
if (n_ix25 + 6 >= 80) { } else {
uint8_t n_deref26 = n_ref1[n_ix25];
ASSERTS(n_ix25 > 0 && 2147483641 >= n_ix25 || n_ix25 <= 0);
ASSERTS(0 <= 6 + n_ix25 && 6 + n_ix25 < 80);
*&n_var2[(6 + n_ix25) % 80] = n_deref26;
}
}
mavlinkSendWithWriter(63U, 130U, 46U, n_var1, n_var2);
for (int32_t n_ix27 = 0; n_ix27 <= 25; n_ix27++) {
ASSERTS(n_ix27 > 0 && 2147483647 - n_ix27 >= 54 || n_ix27 <= 0);
ASSERTS(0 <= n_ix27 + 54 && n_ix27 + 54 < 80);
*&n_var2[(n_ix27 + 54) % 80] = 0U;
}
return;
}
