/* Marzullo implmentation *//* Written by Kin Sun Ho (ksho@cse) *//* Algorithms by Tatiana Bokareva (tbokareva@cse) *//* Last Modified: 06 September 2005 */includes Sasha;module MarzulloM{  provides interface Marzullolib;  provides interface StdControl;  uses {    interface Leds;  }}implementation{  uint16_t id[MAX_MOTES]; // a array of mote_id  struct Interval *input[2*4*MAX_MOTES]; // input intervals from the mote  struct Interval *final[2*4*MAX_MOTES]; // final intervals after Marzullo  uint8_t Nmotes;             // number of motes received  uint8_t Mmotes;             // number of space allocated in memory  uint8_t Mintervals;         // MAX intervals we can have  uint8_t Nintervals;         // # of intervals we have  uint8_t Mfinals;            // MAX final intervals we can have  uint8_t Nfinals;            // # of final intervals we have  uint32_t sum_of_prob;       // tmp parameter  // copy intervals from raw pkt to structure we want  void copyIntervals(Cluster_Msg *info) {        atomic {      input[Nintervals]->pt1  = info->min1;      input[Nintervals]->pt2  = info->max1;      input[Nintervals]->type = 0;      input[Nintervals]->prob = info->win1*100;            input[Nintervals+1]->pt1  = info->max1;      input[Nintervals+1]->pt2  = info->min1;      input[Nintervals+1]->type = 1;      input[Nintervals+1]->prob = 0;            Nintervals+=2;            input[Nintervals]->pt1  = info->min2;      input[Nintervals]->pt2  = info->max2;      input[Nintervals]->type = 0;      input[Nintervals]->prob = info->win2*100;            input[Nintervals+1]->pt1  = info->max2;      input[Nintervals+1]->pt2  = info->min2;      input[Nintervals+1]->type = 1;      input[Nintervals+1]->prob = 0;            Nintervals+=2;            input[Nintervals]->pt1  = info->min3;      input[Nintervals]->pt2  = info->max3;      input[Nintervals]->type = 0;      input[Nintervals]->prob = info->win3*100;            input[Nintervals+1]->pt1  = info->max3;      input[Nintervals+1]->pt2  = info->min3;      input[Nintervals+1]->type = 1;      input[Nintervals+1]->prob = 0;            Nintervals+=2;            input[Nintervals]->pt1  = info->min4;      input[Nintervals]->pt2  = info->max4;      input[Nintervals]->type = 0;      input[Nintervals]->prob = info->win4*100;            input[Nintervals+1]->pt1  = info->max4;      input[Nintervals+1]->pt2  = info->min4;      input[Nintervals+1]->type = 1;      input[Nintervals+1]->prob = 0;            Nintervals+=2;    }  }    // add the data of a mote into MarzulloM  command result_t Marzullolib.add_mote(Cluster_Msg *rmsg) {    int16_t mote_id;    int8_t i;    sum_of_prob = 0;    // find if a pck from a new mote    mote_id = rmsg->src;    for(i=0; i<Nmotes; i++) {      if(mote_id == id[i])	break;    }    if(i != Nmotes) return FAIL; // drop if not a new pkt       // drop pkt when full    if(Nmotes == Mmotes) return FAIL;        // if new store result    atomic {      id[Nmotes] = mote_id;      sum_of_prob = rmsg->win1 + rmsg->win2 + rmsg->win3 + rmsg->win4;      rmsg->win1 = (rmsg->win1*100)/(sum_of_prob); // normalize the probability      rmsg->win2 = (rmsg->win2*100)/(sum_of_prob);      rmsg->win3 = (rmsg->win3*100)/(sum_of_prob);      rmsg->win4 = (rmsg->win4*100)/(sum_of_prob);      copyIntervals(rmsg);      Nmotes++; // store the data and increase the Nmote count    }    dbg(DBG_USR1,"Mote %d is added\n",id[Nmotes-1]);       return SUCCESS;  }  /* The Bubble sort for the input array */  void bubbleSort(uint8_t length) {    uint8_t i,j;    for(i=length; i>=1;i--) {      for(j=0;j<i-1;j++){ 	if (input[j]->pt1 > input[j+1]->pt1){	  /*Swap the elements*/	  uint16_t pt1  = input[j]->pt1;	  uint16_t pt2  = input[j]->pt2;	  uint8_t  type = input[j]->type;	  uint32_t prob = input[j]->prob;	  	  input[j]->pt1  = input[j+1]->pt1;	  input[j]->pt2  = input[j+1]->pt2;	  input[j]->type = input[j+1]->type;	  input[j]->prob = input[j+1]->prob;	  	  input[j+1]->pt1 = pt1;	  input[j+1]->pt2 = pt2;	  input[j+1]->type = type;	  input[j+1]->prob = prob;	}      }    }  }  /*Bubble sort for the final array */  void bubbleSort2(uint8_t length) {    uint8_t i,j;    for(i=length; i>=1;i--) {      for(j=0;j<i-1;j++){ 	if (final[j]->prob <= final[j+1]->prob){	  /*Swap the elements*/	  uint16_t pt1  = final[j]->pt1;	  uint16_t pt2  = final[j]->pt2;	  uint32_t prob = final[j]->prob;	  	  final[j]->pt1  = final[j+1]->pt1;	  final[j]->pt2  = final[j+1]->pt2;	  final[j]->prob = final[j+1]->prob;	  	  final[j+1]->pt1 = pt1;	  final[j+1]->pt2 = pt2;	  final[j+1]->prob = prob;	}      }    }  }  // add a final interval  void addInterval(uint16_t low, uint16_t upper, uint32_t prob){    uint8_t i;    if(low == upper) return;    for(i=0; i<Nfinals; i++) {      /*check if the interval is already existes*/      if((final[i]->pt1  == low) &&	 (final[i]->pt2  == upper) && 	 (final[i]->prob == prob))	return;    }    if(Nfinals == Mfinals) return;        atomic {      final[Nfinals]->pt1 = low;      final[Nfinals]->pt2 = upper;      final[Nfinals]->prob = prob;      Nfinals++;    }  }    // return the final result to Anycast  command Cluster_Msg * Marzullolib.getfinal(Cluster_Msg *pt) {    pt->train = Nfinals;    pt->src   = Nmotes;        // the storing depend on the number of final intervals    if(Nfinals < 1) return pt;    if(Nfinals >= 1) {      pt->min1 = final[0]->pt1;      pt->max1 = final[0]->pt2;      pt->win1 = final[0]->prob;    }        if(Nfinals >= 2) {      pt->min2 = final[1]->pt1;      pt->max2 = final[1]->pt2;      pt->win2 = final[1]->prob;    }    if(Nfinals >= 3) {      pt->min3 = final[2]->pt1;      pt->max3 = final[2]->pt2;      pt->win3 = final[2]->prob;    }        if(Nfinals >= 4) {      pt->min4 = final[3]->pt1;      pt->max4 = final[3]->pt2;      pt->win4 = final[3]->prob;    }    return pt;  }   // calculate the probability  uint32_t calculateProb(uint16_t low, uint16_t upper, uint8_t c) {    uint8_t j;    uint32_t prob      = 0;    uint16_t l_inte = upper-low; /*length of an intersection*/           for(j=0; j<=c; j++) {      if((input[j]->type == 0) && (input[j]->pt2 > low)) {	prob += (input[j]->prob*l_inte)/(input[j]->pt2-input[j]->pt1);      }    }    return prob/Nmotes;  }  // do the Marzullo Business  command uint8_t Marzullolib.doMarzullo() {    uint8_t i,c,j;    uint16_t low, upper;    uint32_t prob;    uint32_t sum;    uint8_t tosend;    c=0; // initalize the parameters    low=0;    upper=0;    sum = 0;    dbg(DBG_USR1,"Nintervals is %d\n",Nintervals);    bubbleSort(Nintervals); // sort the intervals    for(i=0; i<Nintervals; i++) {      dbg(DBG_USR1,"%d %d %d %d %d\n",i,input[i]->type,	  input[i]->pt1,input[i]->pt2,input[i]->prob);    }    for(i=0; i<Nintervals; i++){      dbg(DBG_USR1,"i is %d\n",i);      if(input[i]->type == 0){	low   = input[i]->pt1;	upper = input[i+1]->pt1;	c = i;      }      else if((input[i]->type == 1) && (input[i-1]->type == 0)){	dbg(DBG_USR1,"low %d, upper %d\n",low,upper);	if(upper <= low){	  /*we encounter the joint intersection  	  min1|------max1|min2-------|max2*/	  low  = input[i]->pt2; upper = input[i]->pt1;  	  //find the bigest minimum for this instesection	  for(j=0;j<Nintervals;j++){	    if(input[j]->type == 0 && input[j]->pt1>=low && 	       input[j]->pt1 < upper)	      low=input[j]->pt1;	  }	  prob = calculateProb(low,upper,c-1);	  dbg(DBG_USR1,"1 New final: %d %d %d\n",low, upper,prob);	  addInterval(low,upper,prob);	  	  low  = input[i-1]->pt1; upper = input[i-1]->pt2; 	  prob = calculateProb(low,upper,c);	  dbg(DBG_USR1,"2 New final: %d %d %d\n",low, upper,prob);	  addInterval(low,upper,prob);	}	else{	  prob = calculateProb(low,upper,c);	  dbg(DBG_USR1,"3 New final: %d %d %d\n",low, upper,prob);	  addInterval(low, upper, prob);	}	prob=0;      }    }    bubbleSort2(Nfinals);    dbg(DBG_USR1,"Nfinal is %d\n",Nfinals);    if(Nfinals > 4) tosend = 4;    else tosend = Nfinals;    // Normalize the probabilities of the sending intervals    for(i=0; i<tosend; i++) sum += final[i]->prob;    for(i=0; i<tosend; i++) final[i]->prob = (final[i]->prob*100)/sum;    //call Leds.greenToggle();    return Nfinals;  }  // Reset all parameters  command result_t Marzullolib.reset() {    uint8_t i;    Mmotes     = MAX_MOTES;    Nmotes     = 0;    Mintervals = 4*MAX_MOTES*2;    Nintervals = 0;    Mfinals    = 4*MAX_MOTES*2;    Nfinals    = 0;    sum_of_prob = 0;    //[tb] clear the final intervals structure    for(i=0; i<Nfinals; i++){      final[i]->prob = 0;      final[i]->pt1 = final[i]->pt2=0;    }    return SUCCESS;  }  // Add a mote for simulation  void add_mote_sim() {    Cluster_Msg *c1;    //Cluster_Msg *c2;    //Cluster_Msg *c3;    c1 = (struct Cluster_Msg *)malloc(sizeof(struct Cluster_Msg));    //c2 = (struct Cluster_Msg *)malloc(sizeof(struct Cluster_Msg));    //c3 = (struct Cluster_Msg *)malloc(sizeof(struct Cluster_Msg));    c1->src  = 7;    c1->win1 = 25;    c1->win2 = 25;    c1->win3 = 25;    c1->win4 = 25;    c1->min1 = 20;    c1->min2 = 22;    c1->min3 = 26;    c1->min4 = 28;    c1->max1 = 24;    c1->max2 = 26;    c1->max3 = 30;    c1->max4 = 34;    /*c2->src  = 13;    c2->win1 = 29;    c2->win2 = 25;    c2->win3 = 22;    c2->win4 = 22;    c2->min1 = 934;    c2->min2 = 939;    c2->min3 = 933;    c2->min4 = 933;    c2->max1 = 939;    c2->max2 = 939;    c2->max3 = 939;    c2->max4 = 939;        c3->src  = 6;    c3->win1 = 42;    c3->win2 = 21;    c3->win3 = 17;    c3->win4 = 17;    c3->min1 = 839;    c3->min2 = 837;    c3->min3 = 839;    c3->min4 = 840;    c3->max1 = 842;    c3->max2 = 842;    c3->max3 = 842;    c3->max4 = 842;*/    call Marzullolib.add_mote(c1);    //call Marzullolib.add_mote(c2);    //call Marzullolib.add_mote(c3);  }    // init the application  command result_t StdControl.init() {    int8_t i;    dbg(DBG_USR1,"Marzullo init called\n");    call Leds.init();        Mmotes = MAX_MOTES;    Nmotes = 0;    Mintervals = 4*MAX_MOTES*2;    Nintervals = 0;    Mfinals = 4*MAX_MOTES*2;    Nfinals = 0;    sum_of_prob = 0;        for(i=0; i<Mintervals; i++)      input[i] = (struct Interval*) malloc(sizeof(struct Interval));    for(i=0; i<Mfinals; i++)      final[i] = (struct Interval*) malloc(sizeof(struct Interval));        // uncomment for simulation in tossim    // comment when deploy on mote    //[tb] testing for tossim    //add_mote_sim();        //call Marzullolib.doMarzullo();    return SUCCESS;  }  command result_t StdControl.start() {    return SUCCESS;  }    command result_t StdControl.stop() {    return SUCCESS;  }}