/*---------------------------------------------------------------------------*/
static int
detect_ack(void)
{
#define INTER_PACKET_INTERVAL              RTIMER_ARCH_SECOND / 5000
#define ACK_LEN 3
#define AFTER_ACK_DETECTECT_WAIT_TIME      RTIMER_ARCH_SECOND / 1000
  rtimer_clock_t wt;
  uint8_t ack_received = 0;
  
  wt = RTIMER_NOW();
  leds_on(LEDS_GREEN);
  while(RTIMER_CLOCK_LT(RTIMER_NOW(), wt + INTER_PACKET_INTERVAL)) { }
  leds_off(LEDS_GREEN);
  /* Check for incoming ACK. */
  if((NETSTACK_RADIO.receiving_packet() ||
      NETSTACK_RADIO.pending_packet() ||
      NETSTACK_RADIO.channel_clear() == 0)) {
    int len;
    uint8_t ackbuf[ACK_LEN + 2];
    
    wt = RTIMER_NOW();
    while(RTIMER_CLOCK_LT(RTIMER_NOW(), wt + AFTER_ACK_DETECTECT_WAIT_TIME)) { }
    
    len = NETSTACK_RADIO.read(ackbuf, ACK_LEN);
    if(len == ACK_LEN) {
      ack_received = 1;
    }
  }
  if(ack_received) {
    leds_toggle(LEDS_RED);
  }
  return ack_received;
}

int sht21_humidity(void)
{
  int val;

  i2c_enable();
  /* For for about 15ms before the SHT11 can be used */
  sht21_timer = RTIMER_NOW();
  while(RTIMER_CLOCK_LT(RTIMER_NOW(), sht21_timer + (RTIMER_SECOND/1000)*15));

  buf[0] = 0xe5;
  i2c_transmitinit(0x40, 1, buf);
  while(!i2c_transferred()) ;

  /* Wait for measurement about 85ms */
  sht21_timer = RTIMER_NOW();
  while(RTIMER_CLOCK_LT(RTIMER_NOW(), sht21_timer + (RTIMER_SECOND/1000)*85));

  i2c_receiveinit(0x40, 3, buf);
  while(!i2c_transferred()) ;

  val = (int)(buf[0]<<24 | buf[1]<<16 | buf[2]<<8 | buf[3]);

//  i2c_disable();
  /* return relative humidity * 100 (0.04 % accuracy) */
  return (-6.0 + (125.0*((val>>16)&0x0000fffc))/0x10000)*100;
}

static int
set_locked(struct rtimer *rtimer, rtimer_clock_t time,
	   rtimer_callback_t func, void *ptr)
{
  struct rtimer **anchor;

  /*
   * RTIMER_ERR_ALREADY_SCHEDULED in rtimer.h suggests we should fail if the
   * timer is already scheduled. However, the original implementation allows
   * timers to be rescheduled with impunity, so we maintain de facto
   * compatibility.
   */
  for (anchor = &next_rtimer; *anchor; anchor = &(*anchor)->next)
    if (*anchor == rtimer) {
      *anchor = rtimer->next;
      break;
    }
  rtimer->time = time;
  rtimer->func = func;
  rtimer->ptr = func;
  rtimer->cancel = 0;

  for (anchor = &next_rtimer; *anchor && RTIMER_CLOCK_LT((*anchor)->time, time);
       anchor = &(*anchor)->next);
  rtimer->next = *anchor;
  *anchor = rtimer;

  if (next_rtimer == rtimer)
    rtimer_arch_schedule(time);
  return RTIMER_OK;
}

/**
 * @brief transmit_epidemic
 */
static void transmit_epidemic(){

    packetbuf_clear();
    
    data_packet_t *dpkt = (data_packet_t*)packetbuf_dataptr();
    
    if(isAnchorFlag){
        dpkt->type = ANCHOR_PKT;
    }else{
        dpkt->type = PROBE_PKT;
    }
    //add sender offset.
    dpkt->offset   = probe_offset;
    dpkt->period   = get_node_period();
    dpkt->src_id   = rimeaddr_node_addr.u8[0];
    
    uint8_t pldSize = 0;
    if(radio_read_flag == 0){
        if(list_access_flag == 0){
            list_access_flag = 1;
            pldSize = neighs_add2payload(&dpkt->data[0], isAnchorFlag, probe_offset);
            list_access_flag = 0;
        }

        //if(pldSize){

            rtimer_clock_t t0;
            uint8_t i = 0, pkt_seen = 0, ccaCounter= 0;

            //ccaCounter= randomint_between(CCA_COUNT_MAX, CCA_COUNT_MAX_TX);

	    ccaCounter = /*CCA_COUNT_MAX +*/ random_int(CCA_COUNT_MAX_TX);
	    //watchdog
	    watchdog_periodic();
	     
            for( i = 0; i < ccaCounter && beacon_2_flag; i++){

		//watchdog
		watchdog_periodic();	
		
		
                t0 = RTIMER_NOW();

                while(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + CCA_CHECK_TIME)){
                    //do nothing.. KUNANGA
                }

                if(NETSTACK_RADIO.channel_clear() == 0){
                    pkt_seen = 1;
                    break; // maybe return here .. collision is expected
                }

            }

            if((pkt_seen == 0) && (radio_read_flag == 0)){
                NETSTACK_RADIO.send((void*)packetbuf_dataptr(),DATAPKT_HDR_LEN + pldSize);
            }
        //} //end of pldSize
    }
}

int uart_receive(uint8_t *buffer, uint32_t count)
{
#ifndef __USE_UART_PORT3__
    fprintf(stderr, "uart: __USE_UART_PORT3__ not defined\n");
    return -1;
#else
    uint32_t i = 0;
    int data = -1;
    rtimer_clock_t start;
    start = RTIMER_NOW();
    for(i = 0; i < count; i++)
    {
        while((data = ringbuf_get(&uart_ring_buffer)) < 0)
        {
            if(current_timeout == NO_TIMEOUT)
                continue;

            if(RTIMER_CLOCK_LT(start + current_timeout, RTIMER_NOW()))
            {
                fprintf(stderr, "uart: Read timed out\n");
                return -1;
            }
        }

        buffer[i] = (uint8_t)data;
    }
    return count;
#endif
}

/*---------------------------------------------------------------------------*/
static void hold_time(rtimer_clock_t interval)
{
  rtimer_clock_t rct;
  rct = RTIMER_NOW();
  while(RTIMER_CLOCK_LT(RTIMER_NOW(), rct+interval)){
  }
}

/*Detecta las condiciones del canal y lo añade o elimina de la blacklist*/
static int 
channel_conditions()
{
	NETSTACK_RADIO.get_value(RADIO_PARAM_CHANNEL,&
wake_up_channel);

	if(NETSTACK_RADIO.channel_clear()!= 0 )
	{

		count = 0;
		/*Channel idle*/
		/*Send the packet and channel's badness metric -1
(metric >=0). After a node sends a wake-up beacon: if CCA
==0 and not valid packet --> retransmit packet. If collision
resolution fails --> channel's badness metric +2*/

		if (badness_metric[wake_up_channel] > 0)
		{
			//If badness_metric<=15 remove channel from blacklist
			if (badness_metric[wake_up_channel] == threshold_Cbad+1)
				blacklist ^= (1<<(wake_up_channel)); //XOR  //I think you should only remove the channel from the blacklist once it has passed T_black.
			/* A node R expires and removes from its channel blacklist the channels that have been on the blacklist for more than Tblack time and resets
			 * the badness metric of such channel to 0. */
			--badness_metric[wake_up_channel];
		}
			
		return 1;

	}
else if (NETSTACK_RADIO.channel_clear()== 0 )
	{
		count++;

		if(count < 3)
		{
			t0 = RTIMER_NOW();
			/*From ContikiMAC*/
			while(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + 
CCA_CHECK_TIME)) {}		/* Then, after waiting this time, it should check again, doesn't it? */
		}
		/*If channel still busy after three such CCA checks:
channel's badness metric +2 and sleep*/
		else
		{
			badness_metric[wake_up_channel]+= 2;
			//If badness_metric>15 include channel in blacklist
			if (badness_metric[wake_up_channel] > threshold_Cbad)
				blacklist |= (1<<(wake_up_channel));
			//Sleep
			return 0;
		}
	}
}

/*---------------------------------------------------------------------------*/
rtimer_clock_t
cc26xx_rtc_get_next_trigger()
{
  rtimer_clock_t ch2 = ti_lib_aon_rtc_compare_value_get(AON_RTC_CH2);

  if(HWREG(AON_RTC_BASE + AON_RTC_O_CHCTL) & AON_RTC_CHCTL_CH0_EN) {
    rtimer_clock_t ch0 = ti_lib_aon_rtc_compare_value_get(AON_RTC_CH2);

    return RTIMER_CLOCK_LT(ch0, ch2) ? ch0 : ch2;
  }

  return ch2;
}

static void next_timer_locked(void)
{
  rtimer_clock_t now = RTIMER_NOW();
  struct rtimer *t;

  while (next_rtimer && !RTIMER_CLOCK_LT(now, next_rtimer->time)) {
    t = next_rtimer;   
    next_rtimer = t->next;
    if (!t->cancel)
      t->func(t, t->ptr);
  }
  if (next_rtimer)
    rtimer_arch_schedule(next_rtimer->time);
}

/*---------------------------------------------------------------------------*/
void
rtimer_run_next(void)
{
  int i, n;
  struct rtimer *t;

  /* Do not run timer if list is empty. */
  if(next == firstempty) {
    return;
  }

  t = rtimers[next];

  /* Increase the pointer to the next rtimer. */
  next = (next + 1) % LIST_SIZE;

  /* Run the rtimer. */
  PRINTF("rtimer_run_next running %p\n", t);
  t->func(t, t->ptr);

  if(next == firstempty) {
    PRINTF("rtimer_run_next: empty rtimer list\n");
    /* The list is empty, no more rtimers to schedule. */
    return;
  }

  /* Find the next rtimer to run. */
  n = next;
  for(i = next; i != firstempty; i = (i + 1) % LIST_SIZE) {
    PRINTF("rtimer_run_next checking %p (%d) against %p (%d)\n",
	   rtimers[i], rtimers[i]->time,
	   rtimers[n], rtimers[n]->time);
    if(RTIMER_CLOCK_LT(rtimers[i]->time, rtimers[n]->time)) {
      n = i;
    }
  }

  PRINTF("rtimer_run_next next rtimer is %d %p (%d)\n",
	 n, rtimers[n], rtimers[n]->time);

  /* Put the next rtimer first in the rtimer list. */
  t = rtimers[next];
  rtimers[next] = rtimers[n];
  rtimers[n] = t;

  PRINTF("rtimer_run_next scheduling %d %p (%d)\n",
	 next, rtimers[next], rtimers[next]->time);

  rtimer_arch_schedule(rtimers[next]->time);
}

/**
 * @brief schedule_fixed
 * @param rt
 * @param next_time
 * @return
 */
static char schedule_fixed(struct rtimer *rt, rtimer_clock_t next_time){

    if(RTIMER_CLOCK_LT(next_time, RTIMER_NOW() + 1)) {
        next_time = RTIMER_NOW() + 3;
    }

    int ret = rtimer_set(&generic_timer, next_time, 1,
                         (void (*)(struct rtimer *, void *))power_cycle, NULL);
    if(ret){
        PRINTF("synchronization failed\n");
    }
    
    return 0;
}

static char
powercycle(struct rtimer *t, void *ptr)
{
    if(is_streaming)
    {
        if(!RTIMER_CLOCK_LT(RTIMER_NOW(), stream_until))
        {
            is_streaming = 0;
            rimeaddr_copy(&is_streaming_to, &rimeaddr_null);
            rimeaddr_copy(&is_streaming_to_too, &rimeaddr_null);
        }
    }

    PT_BEGIN(&pt);

    while(1)
    {
        /* Only wait for some cycles to pass for someone to start sending */
        if(someone_is_sending > 0)
        {
            someone_is_sending--;
        }

        /* If there were a strobe in the air, turn radio on */
        powercycle_turn_radio_on();
        schedule_powercycle(t, xmac_config.on_time);
        PT_YIELD(&pt);

        if(xmac_config.off_time > 0 && !NETSTACK_RADIO.receiving_packet())
        {
            powercycle_turn_radio_off();
            if(waiting_for_packet != 0)
            {
                waiting_for_packet++;
                if(waiting_for_packet > 2)
                {
                    /* We should not be awake for more than two consecutive
                       power cycles without having heard a packet, so we turn off
                       the radio. */
                    waiting_for_packet = 0;
                    powercycle_turn_radio_off();
                }
            }
            schedule_powercycle(t, xmac_config.off_time);
            PT_YIELD(&pt);
        }
    }

    PT_END(&pt);
}

/*---------------------------------------------------------------------------*/
PROCESS_THREAD(myTimer_process, ev, data)
{
  static rtimer_clock_t t0;

  PROCESS_BEGIN();

  printf("I start the timer (1 s.)\n");
  while(1) {
    watchdog_periodic();
    t0 = RTIMER_NOW();
    while(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + RTIMER_SECOND));
    watchdog_periodic();
    timeout_handler();
  }

  PROCESS_END();
}

/*---------------------------------------------------------------------------*/
static void
schedule_powercycle_fixed(struct rtimer *t, rtimer_clock_t fixed_time)
{
  int r;

  if(contikimac_is_on) {

    if(RTIMER_CLOCK_LT(fixed_time, RTIMER_NOW() + 1)) {
      fixed_time = RTIMER_NOW() + 1;
    }

    r = rtimer_set(t, fixed_time, 1,
                   (void (*)(struct rtimer *, void *))powercycle, NULL);
    if(r != RTIMER_OK) {
      PRINTF("schedule_powercycle: could not set rtimer\n");
    }
  }
}

static void
schedule_powercycle(struct rtimer *t, rtimer_clock_t time)
{
  int r;

  if(contikimac_is_on) {

    if(RTIMER_CLOCK_LT(RTIMER_TIME(t) + time, RTIMER_NOW() + 2)) {
      time = RTIMER_NOW() - RTIMER_TIME(t) + 2;
    }

    r = rtimer_set(t, RTIMER_TIME(t) + time, 1,
                   (void (*)(struct rtimer *, void *))powercycle, NULL);
    if(r != RTIMER_OK) {
      PRINTF("schedule_powercycle: could not set rtimer\n");
    }
  }
}

/*---------------------------------------------------------------------------*/
PROCESS_THREAD(energy_scan, ev, data)
{

  PROCESS_BEGIN();

  printf("Energy Scanner\n");
  printf("CCA Threshold: %d\n", (int8_t)RSSIH);
  printf("Channel scan range: [%u , %u]\n", CHANNEL_MIN, CHANNEL_MAX);
  printf("%u samples per channel, interval %u ticks\n",
      RSSI_SAMPLES, SAMPLE_INTERVAL);

  channel = CHANNEL_MIN;
  while(1) {
    cmax = RSSI_BASE;
    cc2430_rf_channel_set(channel);
    clock_delay(200);

    for(j = 0; j < RSSI_SAMPLES; j++) {
      t0 = RTIMER_NOW();
      rssi = RSSIL;
      if(rssi > cmax) {
        cmax = rssi;
      }
      while(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + 25));
    }
    printf("%u [%3d]: ", channel, cmax);
    for(j = RSSI_BASE; j <= cmax; j++) {
      printf("#");
    }
    printf("\n");
    if(channel == CHANNEL_MAX) {
      printf("===============\n");
      channel = CHANNEL_MIN;
    } else {
      channel++;
    }

    etimer_set(&et, SAMPLE_INTERVAL);
    PROCESS_YIELD();

  }

  PROCESS_END();
}

/*---------------------------------------------------------------------------*/
static void
schedule_powercycle_fixed(struct rtimer *t, rtimer_clock_t fixed_time)
{
  int r;
  rtimer_clock_t now;

  if(contikimac_is_on) {

    now = RTIMER_NOW();
    if(RTIMER_CLOCK_LT(fixed_time, now + RTIMER_GUARD_TIME)) {
      fixed_time = now + RTIMER_GUARD_TIME;
    }

    r = rtimer_set(t, fixed_time, 1, powercycle_wrapper, NULL);
    if(r != RTIMER_OK) {
      PRINTF("schedule_powercycle: could not set rtimer\n");
    }
  }
}

/*---------------------------------------------------------------------------*/
static char
cpowercycle(void *ptr)
{
  if(is_streaming) {
    if(!RTIMER_CLOCK_LT(RTIMER_NOW(), stream_until)) {
      is_streaming = 0;
      rimeaddr_copy(&is_streaming_to, &rimeaddr_null);
      rimeaddr_copy(&is_streaming_to_too, &rimeaddr_null);
    }
  }

  PT_BEGIN(&pt);

  while(1) {
    /* Only wait for some cycles to pass for someone to start sending */
    if(someone_is_sending > 0) {
      someone_is_sending--;
    }

    /* If there were a strobe in the air, turn radio on */
    powercycle_turn_radio_on();
    CSCHEDULE_POWERCYCLE(DEFAULT_ON_TIME);
    PT_YIELD(&pt);

    if(cxmac_config.off_time > 0) {
      powercycle_turn_radio_off();
      if(waiting_for_packet != 0) {
	waiting_for_packet++;
	if(waiting_for_packet > 2) {
	  /* We should not be awake for more than two consecutive
	     power cycles without having heard a packet, so we turn off
	     the radio. */
	  waiting_for_packet = 0;
	  powercycle_turn_radio_off();
	}
      }
      CSCHEDULE_POWERCYCLE(DEFAULT_OFF_TIME);
      PT_YIELD(&pt);
    }
  }

  PT_END(&pt);
}

/*---------------------------------------------------------------------------*/
static int
configure(int type, int c)
{
  switch(type) {
  case SENSORS_ACTIVE:
    if(c) {
      if(!status(SENSORS_ACTIVE)) {
        rtimer_clock_t t0;
	sht11_init();
        state = ON;

        /* For for about 11 ms before the SHT11 can be used. */
        t0 = RTIMER_NOW();
        while(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + RTIMER_SECOND / 100));
      }
    } else {
      sht11_off();
      state = OFF;
    }
  }
  return 0;
}

static void
schedule_powercycle_fixed(struct rtimer *t, rtimer_clock_t fixed_time)
{
  int r;

  if(contikimac_is_on) {

    if(RTIMER_CLOCK_LT(fixed_time, RTIMER_NOW() + 1)) {
      fixed_time = RTIMER_NOW() + 1;
    }

#if NURTIMER
    r = rtimer_reschedule(t, RTIMER_TIME(t) - time, 1);
#else
    r = rtimer_set(t, fixed_time, 1,
                   (void (*)(struct rtimer *, void *))powercycle, NULL);
#endif
    if(r != RTIMER_OK) {
      printf("schedule_powercycle: could not set rtimer\n");
    }
  }
}