/* This function goes through all encounters to see if it finds a
   matching neighbor. If so, we set a ctimer that will turn on the
   radio just before we expect the neighbor to send a probe packet. If
   we cannot find a matching encounter, we just turn on the radio.

   The outbound packet is put on either the pending_packets_list or
   the queued_packets_list, depending on if the packet should be sent
   immediately.
*/
static void
turn_radio_on_for_neighbor(rimeaddr_t *neighbor, struct queue_list_item *i)
{
  struct encounter *e;

#if WITH_STREAMING
  if(packetbuf_attr(PACKETBUF_ATTR_PACKET_TYPE) ==
     PACKETBUF_ATTR_PACKET_TYPE_STREAM) {
    is_streaming = 1;
    turn_radio_on();
    list_add(queued_packets_list, i);
    ctimer_set(&stream_off_timer, STREAM_OFF_TIME,
	       stream_off, NULL);
    return;
  }
#endif /* WITH_STREAMING */
  
  if(rimeaddr_cmp(neighbor, &rimeaddr_null)) {
#if ! WITH_PENDING_BROADCAST
    /* We have been asked to turn on the radio for a broadcast, so we
       just turn on the radio. */
    turn_radio_on();
#endif /* ! WITH_PENDING_BROADCAST */
    list_add(queued_packets_list, i);
    return;
  }

#if WITH_ENCOUNTER_OPTIMIZATION
  /* We go through the list of encounters to find if we have recorded
     an encounter with this particular neighbor. If so, we can compute
     the time for the next expected encounter and setup a ctimer to
     switch on the radio just before the encounter. */
  for(e = list_head(encounter_list); e != NULL; e = e->next) {
    if(rimeaddr_cmp(neighbor, &e->neighbor)) {
      clock_time_t wait, now;

      /* We expect encounters to happen roughly every OFF_TIME time
	 units. The next expected encounter is at time e->time +
	 OFF_TIME. To compute a relative offset, we subtract with
	 clock_time(). Because we are only interested in turning on
	 the radio within the OFF_TIME period, we compute the waiting
	 time with modulo OFF_TIME. */

      now = clock_time();
      wait = ((clock_time_t)(e->time - now)) % (OFF_TIME);

      /*      printf("now %d e %d e-n %d w %d %d\n", now, e->time, e->time - now, (e->time - now) % (OFF_TIME), wait);
      
      printf("Time now %lu last encounter %lu next expected encouter %lu wait %lu/%d (%lu)\n",
	     (1000ul * (unsigned long)now) / CLOCK_SECOND,
	     (1000ul * (unsigned long)e->time) / CLOCK_SECOND,
	     (1000ul * (unsigned long)(e->time + OFF_TIME)) / CLOCK_SECOND,
	     (1000ul * (unsigned long)wait) / CLOCK_SECOND, wait,
	     (1000ul * (unsigned long)(wait + now)) / CLOCK_SECOND);*/
      
      /*      printf("Neighbor %d.%d found encounter, waiting %d ticks\n",
	      neighbor->u8[0], neighbor->u8[1], wait);*/
      
      ctimer_set(&e->turn_on_radio_timer, wait, turn_radio_on_callback, i);
      list_add(pending_packets_list, i);
      return;
    }
  }
#endif /* WITH_ENCOUNTER_OPTIMIZATION */
  
  /* We did not find the neighbor in the list of recent encounters, so
     we just turn on the radio. */
  /*  printf("Neighbor %d.%d not found in recent encounters\n",
      neighbor->u8[0], neighbor->u8[1]);*/
  turn_radio_on();
  list_add(queued_packets_list, i);
  return;
}

/*---------------------------------------------------------------------------*/
static
PT_THREAD(generate_index(struct httpd_state *s))
{
  static int i;

#if BUF_USES_STACK
  char buf[BUF_SIZE];
#endif
#if WEBSERVER_CONF_LOADTIME
  static clock_time_t numticks;

  numticks = clock_time();
#endif

  PSOCK_BEGIN(&s->sout);

  SEND_STRING(&s->sout, TOP);
  SEND_STRING(&s->sout, BODY);
  reset_buf();
  add_div_home("Info");
  add("<div id=\"left_home\">");
  add("<h2>Info</h2>");
  add("Version : " CETIC_6LBR_VERSION " (" CONTIKI_VERSION_STRING ")<br />");
  add("Mode : ");
#if CETIC_6LBR_SMARTBRIDGE
  add("SMART BRIGDE");
#endif
#if CETIC_6LBR_TRANSPARENTBRIDGE
#if CETIC_6LBR_LEARN_RPL_MAC
  add("RPL Relay");
#else
  add("FULL TRANSPARENT BRIGDE");
#endif
#endif
#if CETIC_6LBR_ROUTER
#if UIP_CONF_IPV6_RPL
  add("RPL ROUTER");
#else
  add("NDP ROUTER");
#endif
#endif
#if CETIC_6LBR_6LR
  add("6LR");
#endif
  add("<br />\n");
  i = clock_seconds() - cetic_6lbr_startup;
  add("Uptime : %dh %dm %ds<br />", i / 3600, (i / 60) % 60, i % 60);
  SEND_STRING(&s->sout, buf);
  reset_buf();

  add("<br /><h2>WSN</h2>");
  add("MAC: %s<br />RDC: %s (%d Hz)<br />",
      NETSTACK_MAC.name,
      NETSTACK_RDC.name,
      (NETSTACK_RDC.channel_check_interval() ==
       0) ? 0 : CLOCK_SECOND / NETSTACK_RDC.channel_check_interval());
#if UIP_CONF_IPV6_RPL
  add("Prefix : ");
  ipaddr_add(&cetic_dag->prefix_info.prefix);
  add("/%d", cetic_dag->prefix_info.length);
  add("<br />");
#endif
  add("HW address : ");
  lladdr_add(&uip_lladdr);
  add("<br />");
  SEND_STRING(&s->sout, buf);
  reset_buf();

  add("<br /><h2>Ethernet</h2>");
#if CETIC_6LBR_ROUTER
  add("Address : ");
  ipaddr_add(&eth_ip_addr);
  add("<br />");
  add("Local address : ");
  ipaddr_add(&eth_ip_local_addr);
  add("<br />");
#endif
  add("HW address : ");
  ethaddr_add(&eth_mac_addr);
  add("<br />");
  SEND_STRING(&s->sout, buf);
  reset_buf();

#if CONTIKI_TARGET_ECONOTAG
  add("<br /><h2>Memory</h2>");

  add("Global : %d (%d %%)<br /><br />", &_end - &_start,
      (100 * (&_end - &_start)) / (96 * 1024));

  add("Code : %d<br />", &_etext - &_start);
  add("Initialised data : %d<br /><br />", &_edata - &_etext);
  add("Data : %d<br />", &_bss_end__ - &__bss_start);
  add("Stack : %d<br />", &__bss_start - &_edata);
  add("Heap : %d<br />", &_end - &_bss_end__);
  SEND_STRING(&s->sout, buf);
  reset_buf();
#endif

  add_div_footer();
//.........这里部分代码省略.........

/* Wait for a multiple of clock ticks (7.8 ms at 128 Hz). */
void clock_wait(clock_time_t t)
{
  clock_time_t start;
  start = clock_time();
  while(clock_time() - start < (clock_time_t)t);
}

/**
 * Restart the timer from the current point in time
 *
 * This function restarts a timer with the same interval that was
 * given to the timer_set() function. The timer will start at the
 * current time.
 *
 * \note A periodic timer will drift if this function is used to reset
 * it. For preioric timers, use the timer_reset() function instead.
 *
 * \param t A pointer to the timer.
 *
 * \sa timer_reset()
 */
void
timer_restart(struct timer *t)
{
  t->start = clock_time();
}

/*---------------------------------------------------------------------------*/
void
lpm_drop()
{
  lpm_registered_module_t *module;
  uint8_t max_pm = LPM_MODE_MAX_SUPPORTED;
  uint8_t module_pm;
  clock_time_t next_event;

  uint32_t domains = LOCKABLE_DOMAINS;

  if(RTIMER_CLOCK_LT(soc_rtc_get_next_trigger(),
                     RTIMER_NOW() + STANDBY_MIN_DURATION)) {
    lpm_sleep();
    return;
  }

  /* Collect max allowed PM permission from interested modules */
  for(module = list_head(modules_list); module != NULL;
      module = module->next) {
    if(module->request_max_pm) {
      module_pm = module->request_max_pm();
      if(module_pm < max_pm) {
        max_pm = module_pm;
      }
    }
  }

  /* Check if any events fired during this process. Last chance to abort */
  if(process_nevents()) {
    return;
  }

  /* Drop */
  if(max_pm == LPM_MODE_SLEEP) {
    lpm_sleep();
  } else {
    /* Critical. Don't get interrupted! */
    ti_lib_int_master_disable();

    /*
     * Reschedule AON RTC CH1 to fire an event N ticks before the next etimer
     * event
     */
    next_event = etimer_next_expiration_time();

    if(next_event) {
      next_event = next_event - clock_time();
      soc_rtc_schedule_one_shot(AON_RTC_CH1, RTIMER_NOW() +
          (next_event * (RTIMER_SECOND / CLOCK_SECOND)));
    }

    /*
     * Notify all registered modules that we are dropping to mode X. We do not
     * need to do this for simple sleep.
     *
     * This is a chance for modules to delay us a little bit until an ongoing
     * operation has finished (e.g. uart TX) or to configure themselves for
     * deep sleep.
     *
     * At this stage, we also collect power domain locks, if any.
     * The argument to PRCMPowerDomainOff() is a bitwise OR, so every time
     * we encounter a lock we just clear the respective bits in the 'domains'
     * variable as required by the lock. In the end the domains variable will
     * just hold whatever has not been cleared
     */
    for(module = list_head(modules_list); module != NULL;
        module = module->next) {
      if(module->shutdown) {
        module->shutdown(max_pm);
      }

      /* Clear the bits specified in the lock */
      domains &= ~module->domain_lock;
    }

    /* Pat the dog: We don't want it to shout right after we wake up */
    watchdog_periodic();

    /* Clear unacceptable bits, just in case a lock provided a bad value */
    domains &= LOCKABLE_DOMAINS;

    /*
     * Freeze the IOs on the boundary between MCU and AON. We only do this if
     * PERIPH is not needed
     */
    if(domains & PRCM_DOMAIN_PERIPH) {
      ti_lib_aon_ioc_freeze_enable();
    }

    /*
     * Among LOCKABLE_DOMAINS, turn off those that are not locked
     *
     * If domains is != 0, pass it as-is
     */
    if(domains) {
      ti_lib_prcm_power_domain_off(domains);
    }

    /*
//.........这里部分代码省略.........

static
PT_THREAD(generate_sensor(struct httpd_state *s))
{
  static uip_ipaddr_t ipaddr;
  static node_info_t * node_info = NULL;

  PSOCK_BEGIN(&s->sout);
  if(s->query && strncmp(s->query, "ip=", 3) == 0 && uiplib_ipaddrconv(s->query + 3, &ipaddr) != 0) {
    node_info = node_info_lookup(&ipaddr);
    if(node_info) {
      add("<h2>Info</h2>");
#if CETIC_NODE_CONFIG_HAS_NAME
      if ( node_config_loaded ) {
        add("Name: %s<br />", node_config_get_name(node_config_find_by_ip(&ipaddr)));
      }
#endif
      add("IP: ");
      ipaddr_add(&ipaddr);
      add("<br />");
      add("Model: -<br />");
      add("Parent: ");
#if CETIC_NODE_CONFIG_HAS_NAME
      if (node_config_loaded) {
        add("%s (", node_config_get_name(node_config_find_by_ip(&node_info->ip_parent)));
        ipaddr_add(&node_info->ip_parent);
        add(")</a>");
      } else
#endif
      {
        ipaddr_add(&node_info->ip_parent);
        add("</a>");
      }
      add("<br />");
      add("Downward route: %s<br />", node_info->has_route ? "Yes" : "No");
      SEND_STRING(&s->sout, buf);
      reset_buf();

      add("<br /><h2>Statistics</h2>");
      add("Since : %d s<br />",
          (clock_time() - node_info->stats_start) / CLOCK_SECOND);
      add("Last seen : %d s<br />",
        (clock_time() - node_info->last_seen) / CLOCK_SECOND);
      add("Hop count: %d<br />", node_info->hop_count);
      add("Parent switch: %d<br />", node_info->parent_switch);
      add("<br /><h3>Upstream</h3>");
      add("Last sequence number: %d<br />", node_info->last_up_sequence);
      add("Messages sent: %d<br />", node_info->messages_sent);
      add("Messages lost: %d<br />", node_info->up_messages_lost);
      if(node_info->messages_sent > 0) {
        add("PRR: %.1f%%<br />", 100.0 * (node_info->messages_sent - node_info->up_messages_lost)/node_info->messages_sent);
      } else {
        add("PRR: n/a<br />");
      }
      add("<br /><h3>Downstream</h3>");
      add("Last sequence number: %d<br />", node_info->last_down_sequence);
      add("Messages sent: %d<br />", node_info->replies_sent);
      add("Messages lost: %d<br />", node_info->down_messages_lost);
      if(node_info->replies_sent > 0) {
        add("PRR: %.1f%%<br />", 100.0 * (node_info->replies_sent - node_info->down_messages_lost)/node_info->replies_sent);
      } else {
        add("PRR: n/a<br />");
      }
      SEND_STRING(&s->sout, buf);
      reset_buf();
      add("<br /><h2>Actions</h2>");
      add("<form action=\"reset-stats\" method=\"get\">");
      add("<input type=\"hidden\" name=\"ip\" value=\"");
      ipaddr_add(&node_info->ipaddr);
      add("\"/>");
      add("<input type=\"submit\" value=\"Reset statistics\"/></form><br />");
      add("<form action=\"rm-node\" method=\"get\">");
      add("<input type=\"hidden\" name=\"ip\" value=\"");
      ipaddr_add(&node_info->ipaddr);
      add("\"/>");
      add("<input type=\"submit\" value=\"Delete node\"/></form><br />");
    } else {
      add("Sensor address unknown");
    }
  } else {
    add("Sensor address missing");
  }
  SEND_STRING(&s->sout, buf);
  reset_buf();
  PSOCK_END(&s->sout);
}

void * calc(void *thread) {
    long t = (long) thread;
    int start_index = t * (X_SIZE / THREADS); 
    int end_index = (t != THREADS - 1)?(t + 1) * (X_SIZE / THREADS) - 1: X_SIZE - 1;
    matrix local(phi.size()), next(phi.size(),row(phi[0].size()));
    row zeros;
    for (int i = 0; i < Y_SIZE; i++) {
        zeros.push_back(0);
    }
    double cur_time = 0, a = 0;
    int x, y;
    int counter = 0;
	unsigned long long iters = 0;
    std::cout << "I am thread " << t << ". My start index: " << start_index << ". My end index: " << end_index << std::endl;
	row left_t, mid_t, right_t;
    unsigned long long start = clock_time();
    double mid, right, left, top, bot;
    while (cur_time < T) {
    //for(int i = 0; i < 3; i ++) {
        pthread_barrier_wait(&syn);
#ifndef NOGNUPLOT
        if (t == 0) {
		if (cur_time > (double) T / 4)
			I[25][25] = 0;
		if (cur_time - a > 1e-2) {
			
			a = cur_time;
			std::stringstream convert;
			convert << counter;
			std::string num = convert.str();
			std::ofstream file((data_file_name + num).c_str(), std::ofstream::out|std::ofstream::trunc);
			for (int i = 0; i < phi.size(); i++) {
				for(int j = 0; j < phi[i].size(); j++) {
					file << i << " " << j << " " << phi[i][j] << std::endl;
				}
			}
			file.close();
			counter ++;
			gnuplot << "splot \"" << data_file_name + num << "\"" << "with lines title \"phi\"\n";
		       gnuplot << "pause 0.1" << std::endl;	
		}
	}
            /*out << cur_time << " ";
            for (int i = 0; i < gnuplot_points.size(); i++) {
                x = gnuplot_points[i].first;
                y = gnuplot_points[i].second;
                if (x >= 0 && x < X_SIZE && y >=0 && y < Y_SIZE)
                    out << phi[x][y] << " ";
            }
		}
        }
        out << std::endl;*/
#endif
		cur_time += dt;
        /*for (int i = start_index; i <= end_index; i ++) {
			iters ++;
			mid_t = phi[i];
            if (i == start_index && start_index == 0) {
				left_t = zeros;
				right_t = phi[i+1];
			}
            else if (i == end_index && end_index == X_SIZE - 1) {
				left_t = phi[i - 1];
				right_t = zeros;
			}
            else {
				left_t = phi[i - 1];
				right_t = phi[i + 1];
			}*/
		for(int i = start_index; i <= end_index; i++){ 
			for(int j = 0; j < phi[i].size(); j++){
				mid = phi[i][j];
				top = j == 0 ? 0:phi[i][j-1];
				bot = j == Y_SIZE - 1  ? 0 : phi[i][j+1];
				left = i == start_index && start_index ==0 ? 0 : phi[i-1][j];
				right = i == end_index && end_index == X_SIZE - 1 ? 0 :phi[i+1][j];
				next[i][j] = mid + (top+ bot + left + right - 4 * mid) * dt / (R * C) + I[i][j] * dt /C;
			}
		}
			//next[i] = solve(left_t, mid_t, right_t, i);
        //}
        pthread_barrier_wait(&bar);
        for (int i = start_index; i <=end_index; i++) {
            phi[i] = next[i];
        }
    }
    pthread_exit(NULL);
}

int main(int argc, char **argv) {
    int time_to_modulate;
    int num_threads;
    int x_grid, y_grid;
    const char* short_options = "tpxy";
    int c;
    T = atoi(argv[4]);
    dt = 1e-6;
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
    x_grid = atoi(argv[1]);
    y_grid = atoi(argv[2]);
    num_threads = atoi(argv[3]);
#ifndef NOGNUPLOT
    /*int print_points = atoi(argv[5]);
    std::cout << "Enter " << print_points << " nodes to print in format <m> <n>" << std::endl;
    int x, y;
    for(int i = 0; i < print_points; i++) {
        std::cin >> x >> y;
        gnuplot_points.push_back(std::make_pair(x, y));
    }*/
#endif
    THREADS = num_threads;
    time_to_modulate = 2;
    X_SIZE = x_grid;
    Y_SIZE = y_grid;
    phi.resize(x_grid);
    for(int i = 0; i < phi.size(); i++) {
        phi[i].resize(y_grid);
    }
    I.resize(x_grid);
    for(int i = 0; i < I.size(); i++) {
        I[i].resize(y_grid);
		for(int j = 0; j < y_grid; j++)
			I[i][j] =  0;//rand() % 6 - 3;
    }
    I[25][25] = 2;
#ifndef NOGNUPLOT
    gnuplot << "set size ratio 1\n";
    gnuplot << "set hidden3d\n";
    gnuplot << "set dgrid 50,50\n";
	gnuplot << "set zrange [-3: 3]\n";
    //int res = system("gnuplot -persist cmd");
#endif
   // I[1][1] = 1;
   // I[0][1] = -1;
    pthread_barrier_init(&bar, NULL, THREADS);
    pthread_barrier_init(&val, NULL, THREADS);
    pthread_barrier_init(&syn, NULL, THREADS);
    pthread_mutex_init(&lock, NULL);
    pthread_t *threads = new pthread_t[THREADS];
    unsigned long long start = clock_time();
    for (long i = 0; i < THREADS; i++) {
        if (pthread_create(&threads[i], NULL, calc, (void *)i) != 0) {
            std::cout << "Can't create thread " << i << std::endl;
        }
    }
    for (int i = 0; i < THREADS; i++) {
        pthread_join(threads[i], NULL);
    }
    std::cout << "It takes " << (clock_time() - start) << std::endl;
    return 0;
}

void
dtls_clock_init(void) {
  clock_init();
  dtls_clock_offset = clock_time();
}

void
dtls_ticks(dtls_tick_t *t) {
  *t = clock_time();
}

/*---------------------------------------------------------------------------*/
PROCESS_THREAD(shell_repeat_process, ev, data)
{
  static int reps, period, period_left;
  static char command[MAX_COMMANDLENGTH];
  static struct etimer etimer;
  static int i;
  static clock_time_t start_time;
  const char *args, *next;

  if(ev == shell_event_input) {
    struct shell_input *input;
    input = data;
    /*    printf("shell repeat input %d %d\n", input->len1, input->len2);*/
    if(input->len1 + input->len2 != 0) {
      shell_output(&repeat_command, input->data1, input->len1,
		   input->data2, input->len2);
    }
  }

  PROCESS_BEGIN();

  /*  printf("data '%s'\n", data);*/

  args = data;

  if(args == NULL) {
    repeat_print_usage();
    PROCESS_EXIT();
  }

  reps = shell_strtolong(args, &next);
  if(next == args) {
    repeat_print_usage();
    PROCESS_EXIT();
  }

  args = next;
  period = shell_strtolong(args, &next);
  if(next == args) {
    repeat_print_usage();
    PROCESS_EXIT();
  }

  args = next;

  while(*args == ' ') {
    args++;
  }

  strncpy(command, args, MAX_COMMANDLENGTH);
  if(strlen(command) == 0) {
    repeat_print_usage();
    PROCESS_EXIT();
  }

  /*  printf("repeats %d period %d command '%s'\n",
      reps, period, command);*/

  start_time = clock_time();
  etimer_set(&etimer, CLOCK_SECOND * period);
  for(i = 0; reps == 0 || i < reps; ++i) {

    process_start(&shell_repeat_server_process, command);
    process_post(&shell_repeat_server_process,
		 PROCESS_EVENT_CONTINUE,
		 &shell_repeat_process);
    PROCESS_WAIT_UNTIL(ev == PROCESS_EVENT_EXITED &&
		       data == &shell_repeat_server_process);
    PROCESS_WAIT_UNTIL(etimer_expired(&etimer));
    etimer_reset(&etimer);
    /*    PROCESS_PAUSE();

    for(period_left = period;
	period_left > 0;
	period_left -= MIN(PERIOD_INTERVAL, period_left)) {
      etimer_set(&etimer, CLOCK_SECOND * MIN(PERIOD_INTERVAL, period_left));
      PROCESS_WAIT_UNTIL(etimer_expired(&etimer));
      }*/
  }
  

  PROCESS_END();
}

/*---------------------------------------------------------------------------*/
uip_ds6_nbr_t *
uip_ds6_nbr_add(uip_ipaddr_t * ipaddr, uip_lladdr_t * lladdr,
                u8_t isrouter, u8_t state)
{
  int r;

  r = uip_ds6_list_loop
     ((uip_ds6_element_t *) uip_ds6_nbr_cache, UIP_DS6_NBR_NB,
      sizeof(uip_ds6_nbr_t), ipaddr, 128,
      (uip_ds6_element_t **) &locnbr);
  //  printf("r %d\n", r);

  if(r == FREESPACE) {
    locnbr->isused = 1;
    uip_ipaddr_copy(&(locnbr->ipaddr), ipaddr);
    if(lladdr != NULL) {
      memcpy(&(locnbr->lladdr), lladdr, UIP_LLADDR_LEN);
    } else {
      memset(&(locnbr->lladdr), 0, UIP_LLADDR_LEN);
    }
    locnbr->isrouter = isrouter;
    locnbr->state = state;
    /* timers are set separately, for now we put them in expired state */
    stimer_set(&(locnbr->reachable), 0);
    stimer_set(&(locnbr->sendns), 0);
    locnbr->nscount = 0;
    PRINTF("Adding neighbor with ip addr");
    PRINT6ADDR(ipaddr);
    PRINTF("link addr");
    PRINTLLADDR((&(locnbr->lladdr)));
    PRINTF("state %u\n", state);
    NEIGHBOR_STATE_CHANGED(locnbr);

    locnbr->last_lookup = clock_time();
    //    printf("add %p\n", locnbr);
    return locnbr;
  } else if(r == NOSPACE) {
    /* We did not find any empty slot on the neighbor list, so we need
       to remove one old entry to make room. */
    uip_ds6_nbr_t *n, *oldest;
    clock_time_t oldest_time;

    oldest = NULL;
    oldest_time = clock_time();

    for(n = uip_ds6_nbr_cache;
        n < &uip_ds6_nbr_cache[UIP_DS6_NBR_NB];
        n++) {
      if(n->isused) {
        if((n->last_lookup < oldest_time) && (uip_ds6_is_nbr_garbage_collectible(n))) {
        	/* We do not want to remove any non-garbage-collectible entry */
          oldest = n;
          oldest_time = n->last_lookup;
        }
      }
    }
    if(oldest != NULL) {
      //      printf("rm3\n");
      uip_ds6_nbr_rm(oldest);
      locdefrt = uip_ds6_defrt_lookup(&oldest->ipaddr);
      uip_ds6_defrt_rm(locdefrt);
      uip_ds6_reg_cleanup_defrt(locdefrt);
			return uip_ds6_nbr_add(ipaddr, lladdr, isrouter, state);
    }
  }
  PRINTF("uip_ds6_nbr_add drop\n");
  return NULL;
}

/**
 * Read a packet from the underlying radio driver. If the incoming
 * packet is a probe packet and the sender of the probe matches the
 * destination address of the queued packet (if any), the queued packet
 * is sent.
 */
static int
read_packet(void)
{
  int len;
  struct lpp_hdr *hdr;
  clock_time_t reception_time;

  reception_time = clock_time();
  
  packetbuf_clear();
  len = radio->read(packetbuf_dataptr(), PACKETBUF_SIZE);
  if(len > sizeof(struct lpp_hdr)) {
    packetbuf_set_datalen(len);
    hdr = packetbuf_dataptr();
    packetbuf_hdrreduce(sizeof(struct lpp_hdr));
    /*    PRINTF("got packet type %d\n", hdr->type);*/

    if(hdr->type == TYPE_PROBE) {
      /* Parse incoming announcements */
      struct announcement_msg *adata = packetbuf_dataptr();
      int i;
	
      /*	PRINTF("%d.%d: probe from %d.%d with %d announcements\n",
		rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
		hdr->sender.u8[0], hdr->sender.u8[1], adata->num);*/
	
      for(i = 0; i < adata->num; ++i) {
	/*	  PRINTF("%d.%d: announcement %d: %d\n",
		  rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
		  adata->data[i].id,
		  adata->data[i].value);*/

	announcement_heard(&hdr->sender,
			   adata->data[i].id,
			   adata->data[i].value);
      }

      /* Register the encounter with the sending node. We now know the
	 neighbor's phase. */
      register_encounter(&hdr->sender, reception_time);

      /* Go through the list of packets to be sent to see if any of
	 them match the sender of the probe, or if they are a
	 broadcast packet that should be sent. */
      if(list_length(queued_packets_list) > 0) {
	struct queue_list_item *i;
	for(i = list_head(queued_packets_list); i != NULL; i = i->next) {
	  struct lpp_hdr *qhdr;
	  
	  qhdr = queuebuf_dataptr(i->packet);
	  if(rimeaddr_cmp(&qhdr->receiver, &hdr->sender) ||
	      rimeaddr_cmp(&qhdr->receiver, &rimeaddr_null)) {
	    queuebuf_to_packetbuf(i->packet);

#if WITH_PENDING_BROADCAST
	    if(i->broadcast_flag == BROADCAST_FLAG_NONE ||
	       i->broadcast_flag == BROADCAST_FLAG_SEND) {
	      radio->send(queuebuf_dataptr(i->packet),
			  queuebuf_datalen(i->packet));
	      PRINTF("%d.%d: got a probe from %d.%d, sent packet to %d.%d\n",
		   rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
		     hdr->sender.u8[0], hdr->sender.u8[1],
		     qhdr->receiver.u8[0], qhdr->receiver.u8[1]);
	      
	    } else {
	      PRINTF("%d.%d: got a probe from %d.%d, did not send packet\n",
		     rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
		     hdr->sender.u8[0], hdr->sender.u8[1]);
	    }
#else /* WITH_PENDING_BROADCAST */
	    radio->send(queuebuf_dataptr(i->packet),
			queuebuf_datalen(i->packet));
	    PRINTF("%d.%d: got a probe from %d.%d, sent packet to %d.%d\n",
		   rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
		   hdr->sender.u8[0], hdr->sender.u8[1],
		   qhdr->receiver.u8[0], qhdr->receiver.u8[1]);
#endif /* WITH_PENDING_BROADCAST */



	    /* Attribute the energy spent on listening for the probe
	       to this packet transmission. */
	    compower_accumulate(&i->compower);
	    
	    /* If the packet was not a broadcast packet, we dequeue it
	       now. Broadcast packets should be transmitted to all
	       neighbors, and are dequeued by the dutycycling function
	       instead, after the appropriate time. */
	    if(!rimeaddr_cmp(&qhdr->receiver, &rimeaddr_null)) {
	      remove_queued_packet(i);

#if WITH_PROBE_AFTER_TRANSMISSION
	      /* Send a probe packet to catch any reply from the other node. */
	      restart_dutycycle(PROBE_AFTER_TRANSMISSION_TIME);
//.........这里部分代码省略.........

/*---------------------------------------------------------------------------*/
static void
receiver(struct simple_udp_connection *c,
         const uip_ipaddr_t *sender_addr,
         uint16_t sender_port,
         const uip_ipaddr_t *receiver_addr,
         uint16_t receiver_port,
	 const uint8_t *data,
         uint16_t datalen)
{
  static int cl_temp;
  static int cl_sec;
  unsigned int clock;
  unsigned int second;
  double temp;

  //printf("recieved msg at clock time: %d \n",clock_time()-  offset - prop_delay);
  //printf( "%d \n",clock_time()-  offset - prop_delay);
    

  TWAMPtimestamp ts_rcv;

  clock = clock_time();
  second = clock/CLOCK_SECOND;
  printf("RECEIVE AT: clock: %d || second: %d \n",clock,second);
  //temp = (double) (clock - start_time)/CLOCK_SECOND - second;
  temp = (double) (clock)/CLOCK_SECOND - second;
  ts_rcv.Second = second;
  ts_rcv.Fraction = temp * 1000;

  printf("Data received from ");  
  uip_debug_ipaddr_print(sender_addr);
  printf(" on port %d from port %d \n", receiver_port, sender_port);
  
  printf("Datalen = %d\n",datalen);
  SenderUAuthPacket sender_pkt;
  memset(&sender_pkt, 0, sizeof(sender_pkt));
  memcpy(&sender_pkt, data, datalen);
  
  ReflectorUAuthPacket reflect_pkt;
  //printf("Paket size = %d\n", sizeof(reflect_pkt));
  reflect_pkt.SeqNo = seqno;
  reflect_pkt.ErrorEstimate = 999;
  reflect_pkt.ReceiverTimestamp = ts_rcv;
  reflect_pkt.SenderSeqNo = sender_pkt.SeqNo;
  reflect_pkt.SenderTimestamp = sender_pkt.Timestamp;
  reflect_pkt.SenderErrorEstimate = sender_pkt.ErrorEstimate;
  reflect_pkt.SenderTTL = 255;


  clock = clock_time();
  second = clock/CLOCK_SECOND;
  temp = (double) (clock)/CLOCK_SECOND - second;
  reflect_pkt.Timestamp.Second = second;
  reflect_pkt.Timestamp.Fraction = temp * 1000;

  simple_udp_sendto(&unicast_connection, &reflect_pkt,
                    sizeof(reflect_pkt), sender_addr);

  printf("Packet reflected to:\n");
  uip_debug_ipaddr_print(sender_addr);
  printf("\n#################\n");

  //Prints for debugging
  printf("SeqNo: %"PRIu32"\n", sender_pkt.SeqNo);
  printf("Seconds: %"PRIu32"\n", sender_pkt.Timestamp.Second);
  printf("Micro: %"PRIu32"\n", sender_pkt.Timestamp.Fraction);
  printf("Error: %"PRIu16"\n", sender_pkt.ErrorEstimate);

  current++;
}

//.........这里部分代码省略.........
  if (!superblock)
	panic("EXT2","Can't allocate memory for superblock.",NO_NUM);
  superblock->s_dev = fs_dev;	/* read_super() needs to know which dev */
  r = read_super(superblock);

  /* Is it recognized as a Minix filesystem? */
  if (r != 0) {
	superblock->s_dev = NO_DEV;
	dev_close(driver_e, fs_dev);
	return(r);
  }

  if (superblock->s_rev_level != EXT2_GOOD_OLD_REV) {
	struct ext2_super_block *sp = superblock; /* just shorter name */
	mask = ~SUPPORTED_INCOMPAT_FEATURES;
	if (HAS_INCOMPAT_FEATURE(sp, mask)) {
		if (HAS_INCOMPAT_FEATURE(sp, INCOMPAT_COMPRESSION & mask))
			printk("ext2: fs compression is not supported by server\n");
		if (HAS_INCOMPAT_FEATURE(sp, INCOMPAT_FILETYPE & mask))
			printk("ext2: fs in dir filetype is not supported by server\n");
		if (HAS_INCOMPAT_FEATURE(sp, INCOMPAT_RECOVER & mask))
			printk("ext2: fs recovery is not supported by server\n");
		if (HAS_INCOMPAT_FEATURE(sp, INCOMPAT_JOURNAL_DEV & mask))
			printk("ext2: fs journal dev is not supported by server\n");
		if (HAS_INCOMPAT_FEATURE(sp, INCOMPAT_META_BG & mask))
			printk("ext2: fs meta bg is not supported by server\n");
		return(-EINVAL);
	}
	mask = ~SUPPORTED_RO_COMPAT_FEATURES;
	if (HAS_RO_COMPAT_FEATURE(sp, mask)) {
		if (HAS_RO_COMPAT_FEATURE(sp, RO_COMPAT_SPARSE_SUPER & mask)) {
			printk("ext2: sparse super is not supported by server, \
				remount read-only\n");
		}
		if (HAS_RO_COMPAT_FEATURE(sp, RO_COMPAT_LARGE_FILE & mask)) {
			printk("ext2: large files are not supported by server, \
				remount read-only\n");
		}
		if (HAS_RO_COMPAT_FEATURE(sp, RO_COMPAT_BTREE_DIR & mask)) {
			printk("ext2: dir's btree is not supported by server, \
				remount read-only\n");
		}
		return(-EINVAL);
	}
  }

  if (superblock->s_state == EXT2_ERROR_FS) {
	printk("ext2: filesystem wasn't cleanly unmounted previous time\n");
        superblock->s_dev = NO_DEV;
	dev_close(driver_e, fs_dev);
	return(-EINVAL);
  }


  set_blocksize(superblock->s_block_size);

  /* Get the root inode of the mounted file system. */
  if ( (root_ip = get_inode(fs_dev, ROOT_INODE)) == NULL)  {
	printk("ext2: couldn't get root inode\n");
	  superblock->s_dev = NO_DEV;
	  dev_close(driver_e, fs_dev);
	  return(-EINVAL);
  }

  if (root_ip != NULL && root_ip->i_mode == 0) {
	  printk("%s:%d zero mode for root inode?\n", __FILE__, __LINE__);
	  put_inode(root_ip);
	  superblock->s_dev = NO_DEV;
	  dev_close(driver_e, fs_dev);
	  return(-EINVAL);
  }

  if (root_ip != NULL && (root_ip->i_mode & I_TYPE) != I_DIRECTORY) {
	printk("%s:%d root inode has wrong type, it's not a DIR\n",
		 __FILE__, __LINE__);
	put_inode(root_ip);
	superblock->s_dev = NO_DEV;
	dev_close(driver_e, fs_dev);
	return(-EINVAL);
  }

  superblock->s_rd_only = readonly;
  superblock->s_is_root = isroot;

  if (!readonly) {
	superblock->s_state = EXT2_ERROR_FS;
	superblock->s_mnt_count++;
	superblock->s_mtime = clock_time();
	write_super(superblock); /* Commit info, we just set above */
  }

  /* Root inode properties */
  fs_m_out.RES_INODE_NR = root_ip->i_num;
  fs_m_out.RES_MODE = root_ip->i_mode;
  fs_m_out.RES_FILE_SIZE_LO = root_ip->i_size;
  fs_m_out.RES_UID = root_ip->i_uid;
  fs_m_out.RES_GID = root_ip->i_gid;

  return(r);
}

void
rt_callback(struct rtimer *t, void *ptr) {
  rt_now = RTIMER_NOW();
  ct = clock_time();
  printf("Task called at %u (clock = %u)\n", rt_now, ct);
}

void
c_lqe_ewma_recv(struct pipe *p, struct stackmodule_i *module){
	PRINTF("c_lqe_ewma_recv seqno %u \n", packetbuf_attr(PACKETBUF_ATTR_PACKET_ID));

	uint16_t crtseqno = packetbuf_attr(PACKETBUF_ATTR_PACKET_ID);
	int16_t l = 0;

	struct c_neighbor *n;
	struct c_neighbor *new_n = memb_alloc(&p->neighbor_mem);;
	rimeaddr_copy(&new_n->addr, get_node_addr(0, 1, 0));
	double alpha = p->lqe_ewma_param.alpha;
	uint8_t list_len = list_length(p->neighbor_list);

	double highest_cost = 100.0;
	/* Find the neighbor. */
	for(n = list_head(p->neighbor_list); n != NULL; n = list_item_next(n)) {
	    if(rimeaddr_cmp(&new_n->addr, &n->addr)) {
	    	if (highest_cost > n->cost) { highest_cost = n->cost; }

	    	PRINTF("node found \n");
	    	PRINTF("n-lastseqno %d n-m %d n-k %d\n", n->last_seq_no, n->m, n->k);

	    	new_n->m = crtseqno - n->last_seq_no - 1; //the current seq number - last seq number - crt packet no
	    	if (new_n->m > n->k) {
	    		l = new_n->m - n->k;
	    		new_n->k = n->k;
	    	} else { l = 0; new_n->k = 0; }

	    	PRINTF("newn-lastseqno %d newn-m %d newn-k %d\n", new_n->last_seq_no, new_n->m, new_n->k);

	    	uint8_t lost_p;
	    	for(lost_p = 0; lost_p < l; lost_p++) {	n->cost *= alpha; }
	    	new_n->cost = n->cost * alpha + (1 - alpha);
	    	PRINTF("l %d cost %f\n", l, new_n->cost);

	    	if (n->count_lock == 1) {
	    		new_n->count = n->count;
	    	} else { new_n->count = n->count + 1; }
	    	new_n->first_time_stamp = n->first_time_stamp;
	    	new_n->last_seq_no = crtseqno;
	    	new_n->last_time_stamp = clock_time();
	    	new_n->rate = (new_n->count + new_n->m) / (new_n->last_time_stamp - new_n->first_time_stamp);
	    	new_n->cost_lock = 1;

	    	list_remove(p->neighbor_list, n);
	    	memb_free(&p->neighbor_mem, n);
	    	list_push(p->neighbor_list, new_n);
	    	print_list(p->neighbor_list);

	    	return;
	    }
	 }

	new_n->count = 1;
	new_n->cost = 1;
	new_n->m = 0;
	new_n->k = 0;
	new_n->last_seq_no = crtseqno;
	new_n->rate = 1 / clock_time();
	new_n->last_time_stamp = clock_time();
	new_n->cost_lock = 1;
	new_n->first_time_stamp = new_n->last_time_stamp;

	/*If neighbor is not in the list and list is full, remove from list before adding*/
	if (list_len >= NUM_NEIGHBOR_ENTRIES) {
		for(n = list_head(p->neighbor_list); n != NULL; n = list_item_next(n)) {
			if (highest_cost == n->cost) {
				list_remove(p->neighbor_list, n);
				memb_free(&p->neighbor_mem, n);
			}
		}
	}
	list_push(p->neighbor_list, new_n);
	print_list(p->neighbor_list);
}

static void set_timer(clock_time_t timeout)
{
	future = clock_time() + timeout;
}

/**
 * The time until the timer expires
 *
 * This function returns the time until the timer expires.
 *
 * \param t A pointer to the timer
 *
 * \return The time until the timer expires
 *
 */
clock_time_t
timer_remaining(struct timer *t)
{
  return t->start + t->interval - clock_time();
}