static void
rx_callback(uint32_t id, uint32_t len)
{
    struct sol_i2c *i2c = buses[id];
    uint32_t offset;
    qm_rc_t ret;
    bool stop;

    if (i2c->xfer.type != READ_REG_MULTIPLE) {
        i2c->xfer.status = len;
        process_post(&soletta_app_process, i2c_irq_event, i2c);
        return;
    }

    if (i2c->xfer.multiple_done == i2c->xfer.multiple_count) {
        i2c->xfer.status = i2c->xfer.length * i2c->xfer.multiple_count;
        process_post(&soletta_app_process, i2c_irq_event, i2c);
        return;
    }

    offset = i2c->xfer.multiple_done * i2c->xfer.length;
    i2c->xfer.multiple_done++;
    stop = i2c->xfer.multiple_done == i2c->xfer.multiple_count;

    ret = begin_transfer(i2c->bus, i2c->slave_addr, id, &i2c->xfer.reg, 1,
        i2c->xfer.data + offset, i2c->xfer.length, stop);

    if (ret == QM_RC_OK)
        return;

    i2c->xfer.status = -ret;
    process_post(&soletta_app_process, i2c_irq_event, i2c);
}

static void
tx_callback(uint32_t id, uint32_t len)
{
    struct sol_i2c *i2c = buses[id];
    qm_rc_t ret;

    if ((i2c->xfer.type != WRITE) && (i2c->xfer.type != WRITE_REG))
        return;

    if (i2c->xfer.multiple_count != i2c->xfer.multiple_done) {
        ret = begin_transfer(i2c->bus, i2c->slave_addr, id, i2c->xfer.data,
            i2c->xfer.length, NULL, 0, true);
        if (ret != QM_RC_OK) {
            i2c->xfer.status = -ret;
            process_post(&soletta_app_process, i2c_irq_event, i2c);
            return;
        }
        i2c->xfer.multiple_done++;
        return;
    }

    i2c->xfer.status = len;

    process_post(&soletta_app_process, i2c_irq_event, i2c);
}

static void pollhandler()
{
    U8 status;

    if (dcb.data_rx)
    {
        process_post(&mac_process, event_mac_rx, NULL);
        dcb.data_rx = false;
    }
    else if (dcb.status_avail)
    {
        switch (dcb.status)
        {
        case RADIO_SUCCESS:
            status = MAC_SUCCESS;
            break;
        case RADIO_NO_ACK:
            status = MAC_NO_ACK;
            break;
        case RADIO_CHANNEL_ACCESS_FAILURE:
            status = MAC_CHANNEL_ACCESS_FAILURE;
            break;
        default:
            status = MAC_UNSPECIFIED_FAILURE;
            break;
        }
        process_post(&mac_process, event_drvr_conf, &dcb);
        dcb.status_avail = false;
    }

    process_poll(&drvr_process);
}

/*---------------------------------------------------------------------------*/
void forwarderActivity() {

	pmesg(200, "%s :: %s :: Line #%d\n", __FILE__, __func__, __LINE__);

	//When the forwarder not initialized, do nothing
	if(!isRunningForwardingEngine) {
		return;
	}
	else if(rootControl_isRoot()) {
		//As soon as we are root, should have zero virtual queue
		virtualQueueSize = 0;

		//The root should always be beaconing
		if(ctimer_expired(&beacon_timer)) {
			// 	If this is the start of the beacon, be agressive! Beacon fast
			//	so neighbors can re-direct rapidly.
			isBeaconTimerPeriodic = false;
			ctimer_set(&beacon_timer, FAST_BEACON_TIME, beacon_timer_fired, NULL);
     		timer_reset(&beacon_timerTime);  // Reset the tandem timer
		}

		//	If there are packets sitting in the forwarding queue, get them to the reciever
		if(list_length(send_stack) == 0) {
			//	 Don't need sending set to true here? Not using radio?
			//	NO_SNOOP: set beacon type
			beaconType = NORMAL_BEACON;

			process_post(&sendDataTask, NULL, NULL);
		}
		return;
	}
	else if (sending) {
		pmesg(100, "ForwardingEngine is already sending\n");
		return;
	}
	else if (!(list_length(send_stack) == 0) || virtualQueueSize > 0 || sendQeOccupied ) {
		//Stop Beacon, not needed if we are sending data packets (snoop)
#ifndef BEACON_ONLY
		ctimer_stop(&beacon_timer);
    // Stop the tandem timer here, but can't because API doesnt provide that functionality
#endif
		//	Start sending a data packet
		sending = true;

		//NO_SNOOP: set beacon type
		beaconType = NORMAL_BEACON;
		process_post(&sendDataTask, NULL, NULL);
		return;
	}
	else{
	//	Nothing to do but start the beacon!
#ifndef BEACON_ONLY
		isBeaconTimerPeriodic = true;
		ctimer_set(&beacon_timer, BEACON_TIME, beacon_timer_fired, NULL);
    	timer_reset(&beacon_timerTime);  // Reset the tandem timer
#endif
	}

	pmesg(200, "%s :: %s :: Line #%d\n", __FILE__, __func__, __LINE__);
}

/*---------------------------------------------------------------------------*/
static int
event(struct dtls_context_t *ctx, session_t *session,
      dtls_alert_level_t level, unsigned short code)
{
  coap_context_t *coap_ctx;
  coap_ctx = dtls_get_app_data(ctx);

  if(coap_ctx == COAP_CONTEXT_NONE || coap_ctx->is_used == 0) {
    /* The context is no longer in use */
  } else if(code == DTLS_EVENT_CONNECTED) {
    coap_ctx->status = STATUS_CONNECTED;
    PRINTF("coap-context: DTLS CLIENT CONNECTED!\n");
    process_post(coap_ctx->process, coap_context_event, coap_ctx);
  } else if(code == DTLS_EVENT_CONNECT || code == DTLS_EVENT_RENEGOTIATE) {
    coap_ctx->status = STATUS_CONNECTING;
    PRINTF("coap-context: DTLS CLIENT NOT CONNECTED!\n");
    process_post(coap_ctx->process, coap_context_event, coap_ctx);
  } else if(level == DTLS_ALERT_LEVEL_FATAL && code < 256) {
    /* Fatal alert */
    if (coap_ctx && coap_ctx->buf) {
      ip_buf_unref(coap_ctx->buf);
      coap_ctx->buf = NULL;
    }
    coap_ctx->status = STATUS_ALERT;
    PRINTF("coap-context: DTLS CLIENT ALERT %u!\n", code);
    process_post(coap_ctx->process, coap_context_event, coap_ctx);
  }
  return 0;
}

void EXTI0_IRQHandler(void)
{
   if(EXTI_GetITStatus(DIO0_IRQ) != RESET)
    {
      EXTI_ClearITPendingBit(DIO0_IRQ);
      
      hal_DIOx_ITConfig(all,DISABLE);
      
      if(g_fsk.states == RF_STATE_TX_RUNNING)
      {
        SX1276FskSetOpMode( RF_OPMODE_STANDBY ); 
        g_fsk.states = RF_STATE_TX_DONE;
        process_post(&hal_RF_process, PROCESS_EVENT_MSG, (void *)(&g_fsk.states));
       
      }
      
      if(g_fsk.states == RF_STATE_RX_SYNC) 
      {
        SX1276FskSetOpMode( RF_OPMODE_STANDBY ); 
        read_fifo(true);
        etimer_stop(&timer_rf);
        g_fsk.states = RF_STATE_RX_DONE;
        process_post(&hal_RF_process, PROCESS_EVENT_MSG, (void *)(&g_fsk.states));
      }
    } 
}

/* ADC10 ISR */
ISR(ADC10, adc10_interrupt)
{
  /* check for how adc was called */
  switch(state) {
  case POLL:
    if(calling_process != NULL) {
      *dest = ADC10MEM;
      process_poll(calling_process);
    }
    break;

  case EVENT:
    adcbuf = ADC10MEM;
    if(calling_process == NULL) {
      /* if no calling process was specified, notify all processes */
      process_post(PROCESS_BROADCAST, adc_event, &adcbuf);
    } else {
      /* just this single process should get the event */
      process_post(calling_process, adc_event, &adcbuf);
    }
    break;

  case ASYNCH:
    *dest = ADC10MEM;
    break;

  case SYNCH:
  default:
    break;
  }

  /* wake the mcu up so the event can be handled */
  state = OFF;
  LPM4_EXIT;
}

/*---------------------------------------------------------------------------*/
static void
weather_meter_interrupt_handler(uint8_t port, uint8_t pin)
{
  uint32_t aux;

  /* Prevent bounce events */
  if(!timer_expired(&debouncetimer)) {
    return;
  }

  timer_set(&debouncetimer, DEBOUNCE_DURATION);

  /* We make a process_post() to check in the pollhandler any specific threshold
   * value
   */

  if((port == ANEMOMETER_SENSOR_PORT) && (pin == ANEMOMETER_SENSOR_PIN)) {
    weather_sensors.anemometer.ticks++;
    process_post(&weather_meter_int_process, anemometer_int_event, NULL);
  } else if((port == RAIN_GAUGE_SENSOR_PORT) && (pin == RAIN_GAUGE_SENSOR_PIN)) {
    weather_sensors.rain_gauge.ticks++;
    aux = weather_sensors.rain_gauge.ticks * WEATHER_METER_AUX_RAIN_MM;
    aux /= 1000;
    weather_sensors.rain_gauge.value = (uint16_t)aux;
    process_post(&weather_meter_int_process, rain_gauge_int_event, NULL);
  }
}

/*---------------------------------------------------------------------------*/
PROCESS_THREAD(serial_line_process, ev, data)
{
  static char buf[BUFSIZE];
  static int ptr;

  PROCESS_BEGIN();

  serial_line_event_message = process_alloc_event();


  ptr = 0;

  while(1) {
    /* Fill application buffer until newline or empty */
    
    printf("Inside serial_line_process : serial_line_event_message = %d\n", serial_line_event_message);
  
    int c = ringbuf_get(&rxbuf);
    
    if(c == -1) {
      printf("rxbuf empty, process yeilding\n") ;
      /* Buffer empty, wait for poll */
      PROCESS_YIELD();
    } else {
      if(c != END) {
        if(ptr < BUFSIZE-1) {
          buf[ptr++] = (uint8_t)c;
        } else {
          /* Ignore character (wait for EOL) */
        }
      } else {
        /* Terminate */
        buf[ptr++] = (uint8_t)'\0';

        /* Broadcast event */
   
       printf("Broadcast event serial_line_event_message\n");
       process_post(PROCESS_BROADCAST, serial_line_event_message, buf);

        /* Wait until all processes have handled the serial line event */
        if(PROCESS_ERR_OK ==
          process_post(PROCESS_CURRENT(), PROCESS_EVENT_CONTINUE, NULL)) {
          PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_CONTINUE);
        }
        ptr = 0;
      }
    }
  }

  PROCESS_END();
}

/*
 * Handles TCP events from Simple TCP
 */
static void
tcp_event(struct tcp_socket *s, void *ptr, tcp_socket_event_t event)
{
  struct mqtt_connection *conn = ptr;

  /* Take care of event */
  switch(event) {

  /* Fall through to manage different disconnect event the same way. */
  case TCP_SOCKET_CLOSED:
  case TCP_SOCKET_TIMEDOUT:
  case TCP_SOCKET_ABORTED: {

    DBG("MQTT - Disconnected by tcp event %d\n", event);
    process_post(&mqtt_process, mqtt_abort_now_event, conn);
    conn->state = MQTT_CONN_STATE_NOT_CONNECTED;
    ctimer_stop(&conn->keep_alive_timer);
    call_event(conn, MQTT_EVENT_DISCONNECTED, &event);
    abort_connection(conn);

    /* If connecting retry */
    if(conn->auto_reconnect == 1) {
      connect_tcp(conn);
    }
    break;
  }
  case TCP_SOCKET_CONNECTED: {
    conn->state = MQTT_CONN_STATE_TCP_CONNECTED;
    conn->out_buffer_sent = 1;

    process_post(&mqtt_process, mqtt_do_connect_mqtt_event, conn);
    break;
  }
  case TCP_SOCKET_DATA_SENT: {
    DBG("MQTT - Got TCP_DATA_SENT\n");

    if(conn->socket.output_data_len == 0) {
      conn->out_buffer_sent = 1;
      conn->out_buffer_ptr = conn->out_buffer;
    }

    ctimer_restart(&conn->keep_alive_timer);
    break;
  }

  default: {
    DBG("MQTT - TCP Event %d is currently not managed by the tcp event callback\n",
        event);
  }
  }
}

PROCESS_THREAD(sdcard_process, ev , data)
{
  static struct etimer timer;
  PROCESS_BEGIN();
  *AT91C_PIOA_PER = AT91C_PIO_PA20 | AT91C_PIO_PA1;
  *AT91C_PIOA_ODR = AT91C_PIO_PA20 | AT91C_PIO_PA1;
  
  
  /* Card not inserted */
  sdcard_efs.myCard.sectorCount = 0;
  init_spi();
  
  while(1) {
    if (!(*AT91C_PIOA_PDSR & AT91C_PA20_IRQ0)) {
      if (sdcard_efs.myCard.sectorCount == 0) {
	if (efs_init(&sdcard_efs,0) == 0) {
	  if (event_process) {
	    process_post(event_process, sdcard_inserted_event, NULL);
	  }
	  printf("SD card inserted\n");
	} else {
	  printf("SD card insertion failed\n");
	}
      }
    } else {
      if (sdcard_efs.myCard.sectorCount != 0) {
	/* Card removed */
	  fs_umount(&sdcard_efs.myFs);
	  sdcard_efs.myCard.sectorCount = 0;
	  if (event_process) {
	    process_post(event_process, sdcard_removed_event, NULL);
	  }
	  printf("SD card removed\n");
      }
    }
    
    etimer_set(&timer, CLOCK_SECOND);
    PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_EXIT ||
			     ev == PROCESS_EVENT_TIMER);
    if (ev == PROCESS_EVENT_EXIT) break;
    if (!(*AT91C_PIOA_PDSR & AT91C_PA20_IRQ0)) {
      /* Wait for card to be preperly inserted */
      etimer_set(&timer,CLOCK_SECOND/2);
      PROCESS_WAIT_EVENT_UNTIL(ev== PROCESS_EVENT_TIMER);
    }
    
  }
  PROCESS_END();
}

PROCESS_THREAD(sdcard_process, ev , data)
{
  int fd;
  static struct etimer timer;
  PROCESS_BEGIN();
  /* Mark all file descriptors as free */
  for (fd = 0; fd < MAX_FDS; fd++) {
    file_setAttr(&file_descriptors[fd], FILE_STATUS_OPEN,0);
  }
  /* Card not inserted */
  sdcard_efs.myCard.sectorCount = 0;
  init_spi();

  etimer_set(&timer, CLOCK_SECOND);
  while(1) {
    PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_EXIT ||
			     ev== PROCESS_EVENT_TIMER || ev == PROCESS_EVENT_POLL);
    if (ev == PROCESS_EVENT_EXIT) break;
    if (ev == PROCESS_EVENT_TIMER) {
      if (!(GPIOA->IDR & (1<<0))) {
	if (sdcard_efs.myCard.sectorCount == 0) {
	  etimer_set(&timer,CLOCK_SECOND/2);
	  PROCESS_WAIT_EVENT_UNTIL(ev== PROCESS_EVENT_TIMER);
	  if (efs_init(&sdcard_efs,0) == 0) {
	    if (event_process) {
	      process_post(event_process, sdcard_inserted_event, NULL);
	    }
	    printf("SD card inserted\n");
	  } else {
	    printf("SD card insertion failed\n");
	  }
	}
      } else {
	if (sdcard_efs.myCard.sectorCount != 0) {
	  /* Card removed */
	  fs_umount(&sdcard_efs.myFs);
	  sdcard_efs.myCard.sectorCount = 0;
	  if (event_process) {
	    process_post(event_process, sdcard_removed_event, NULL);
	  }
	  printf("SD card removed\n");
	}
      }
      etimer_set(&timer, CLOCK_SECOND);
      
    }
  }
  PROCESS_END();
}

void
accm_ff_cb(u8_t reg){
  L_ON(LEDS_B);
  process_post(&led_process, ledOff_event, NULL);
  printf("~~[%u] Freefall detected! (0x%02X) -- ", ((u16_t) clock_time())/128, reg);
  print_int(reg);
}

void cack_handler(ChannelState *state, DataPayload *dp){
	if (state->state != STATE_CONNECT){
		PRINTF("Not in Connecting state\n");
		return;
	}
	ConnectACKMsg *ck = (ConnectACKMsg*)dp->data;
	if (ck->accept == 0){
		PRINTF("SCREAM! THEY DIDN'T EXCEPT!!");
	}
	PRINTF("%s accepts connection request on channel %d\n",ck->name,dp->hdr.src_chan_num);
	state->remote_chan_num = dp->hdr.src_chan_num;

	DataPayload *new_dp = &(state->packet);
	clean_packet(new_dp);
	new_dp->hdr.src_chan_num = state->chan_num;
	new_dp->hdr.dst_chan_num = state->remote_chan_num;
	//dp_complete(new_dp,10,QACK,1);
    (new_dp)->hdr.cmd = CACK; 
    (new_dp)->dhdr.tlen = UIP_HTONS(0);
	send_on_knot_channel(state,new_dp);
	state->state = STATE_CONNECTED;
	state->ticks = 100;
	DeviceInfo di;
	strcpy(di.name, ck->name);
	di.channelID = state->chan_num; 
	process_post(state->ccb.client_process, KNOT_EVENT_CONNECTED_DEVICE, &di);
	printf("Timeout in %ds\n",state->rate * PING_WAIT );
	ctimer_set(&(state->timer),CLOCK_CONF_SECOND * (state->rate * PING_WAIT) ,ping_callback, state); 
}

void
rpl_update_forwarder_set(rpl_instance_t *instance)
{
  if(!we_are_root) {
    process_post(&rpl_forwarder_set_update_process, PROCESS_EVENT_CONTINUE, instance);
  }
}

static uip_ipaddr_t *
lookup(uip_ipaddr_t *destipaddr, uip_ipaddr_t *nexthop)
{
  static rimeaddr_t rimeaddr;
  struct route_entry *route;
  int i;

  for(i = 1; i < sizeof(rimeaddr); i++) {
    rimeaddr.u8[i] = destipaddr->u8[sizeof(*destipaddr) - sizeof(rimeaddr) + i];
  }
  rimeaddr.u8[0] = 0;

  PRINTF("rimeroute: looking up ");
  PRINT6ADDR(destipaddr);
  PRINTF(" with Rime address ");
  PRINTRIMEADDR((&rimeaddr));
  PRINTF("\n");

  route = route_lookup(&rimeaddr);
  if(route == NULL) {
    process_post(&rimeroute_process, rimeroute_event, &rimeaddr);
    return NULL;
  }

  uip_ip6addr(nexthop, 0xfe80, 0, 0, 0, 0, 0, 0, 0);
  uip_netif_addr_autoconf_set(nexthop, (uip_lladdr_t *)&route->nexthop);
  PRINTF("rimeroute: ");
  PRINT6ADDR(destipaddr);
  PRINTF(" can be reached via ");
  PRINT6ADDR(nexthop);
  PRINTF("\n");

  return nexthop;
}

static int
read_chunk(struct rucb_conn *c, int offset, char *to, int maxsize)
{
  int size;
  /*  printf("-");*/
  size = maxsize;
  if(bytecount + maxsize >= filesize) {
    size = filesize - bytecount;
  }
  if(size > packetsize) {
    size = packetsize;
  }
  bytecount += size;
  if(bytecount == filesize) {
    end_time_rucb = clock_time();
    download_complete = 1;
    process_post(&shell_sendtest_process, PROCESS_EVENT_CONTINUE, NULL);
    /*     profile_aggregates_print(); */
/*     profile_print_stats(); */
//    print_stats();
  }

  /*  printf("bytecount %lu\n", bytecount);*/
  return size;
}

/*
 * Connect to MQTT broker.
 *
 * N.B. Non-blocking call.
 */
mqtt_status_t
mqtt_connect(struct mqtt_connection *conn, char *host, uint16_t port,
             uint16_t keep_alive)
{
  uip_ip6addr_t ip6addr;
  uip_ipaddr_t *ipaddr;
  ipaddr = &ip6addr;

  /* Check if we are already trying to connect */
  if(conn->state > MQTT_CONN_STATE_NOT_CONNECTED) {
    return MQTT_STATUS_OK;
  }

  conn->server_host = host;
  conn->keep_alive = keep_alive;
  conn->server_port = port;
  conn->out_buffer_ptr = conn->out_buffer;
  conn->out_packet.qos_state = MQTT_QOS_STATE_NO_ACK;
  conn->connect_vhdr_flags |= MQTT_VHDR_CLEAN_SESSION_FLAG;

  /* convert the string IPv6 address to a numeric IPv6 address */
  uiplib_ip6addrconv(host, &ip6addr);

  uip_ipaddr_copy(&(conn->server_ip), ipaddr);

  /*
   * Initiate the connection if the IP could be resolved. Otherwise the
   * connection will be initiated when the DNS lookup is finished, in the main
   * event loop.
   */
  process_post(&mqtt_process, mqtt_do_connect_tcp_event, conn);

  return MQTT_STATUS_OK;
}

/*----------------------------------------------------------------------------*/
mqtt_status_t
mqtt_publish(struct mqtt_connection *conn, uint16_t *mid, char *topic,
             uint8_t *payload, uint32_t payload_size,
             mqtt_qos_level_t qos_level, mqtt_retain_t retain)
{
  if(conn->state != MQTT_CONN_STATE_CONNECTED_TO_BROKER) {
    return MQTT_STATUS_NOT_CONNECTED_ERROR;
  }

  DBG("MQTT - Call to mqtt_publish...\n");

  /* Currently don't have a queue, so only one item at a time */
  if(conn->out_queue_full) {
    DBG("MQTT - Not accepted!\n");
    return MQTT_STATUS_OUT_QUEUE_FULL;
  }
  conn->out_queue_full = 1;
  DBG("MQTT - Accepted!\n");

  conn->out_packet.mid = INCREMENT_MID(conn);
  conn->out_packet.retain = retain;
  conn->out_packet.topic = topic;
  conn->out_packet.topic_length = strlen(topic);
  conn->out_packet.payload = payload;
  conn->out_packet.payload_size = payload_size;
  conn->out_packet.qos = qos_level;
  conn->out_packet.qos_state = MQTT_QOS_STATE_NO_ACK;

  process_post(&mqtt_process, mqtt_do_publish_event, conn);
  return MQTT_STATUS_OK;
}

struct uaodv_rt_entry *
uaodv_request_route_to(uip_ipaddr_t *host)
{
  struct uaodv_rt_entry *route = uaodv_rt_lookup(host);

  if(route != NULL) {
    uaodv_rt_lru(route);
    return route;
  }

  /*
   * Broadcast protocols must be rate-limited!
   */
  if(!timer_expired(&next_time)) {
    return NULL;
  }

  if(command != COMMAND_NONE) {
    return NULL;
  }

  uip_ipaddr_copy(&rreq_addr, host);
  command = COMMAND_SEND_RREQ;
  process_post(&uaodv_process, PROCESS_EVENT_MSG, NULL);
  timer_set(&next_time, CLOCK_SECOND/8); /* Max 10/s per RFC3561. */
  return NULL;
}