/*---------------------------------------------------------------------------*/
void
mac_sequence_register_seqno(void)
{
  int i, j;

  /* Locate possible previous sequence number for this address. */
  for(i = 0; i < MAX_SEQNOS; ++i) {
    if(linkaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_SENDER),
                    &received_seqnos[i].sender)) {
      i++;
      break;
    }
  }

  /* Keep the last sequence number for each address as per 802.15.4e. */
  for(j = i - 1; j > 0; --j) {
    memcpy(&received_seqnos[j], &received_seqnos[j - 1], sizeof(struct seqno));
  }
  received_seqnos[0].seqno = packetbuf_attr(PACKETBUF_ATTR_PACKET_ID);
  linkaddr_copy(&received_seqnos[0].sender,
                packetbuf_addr(PACKETBUF_ADDR_SENDER));
}

/*---------------------------------------------------------------------------*/
static void
recv_from_abc(struct abc_conn *bc)
{
  rimeaddr_t sender;
  struct broadcast_conn *c = (struct broadcast_conn *)bc;

  rimeaddr_copy(&sender, packetbuf_addr(PACKETBUF_ADDR_SENDER));
  
  PRINTF("%d.%d: broadcast: from %d.%d\n",
	 rimeaddr_node_addr.u8[0],rimeaddr_node_addr.u8[1],
	 sender.u8[0], sender.u8[1]);
  c->u->recv(c, &sender);
}

//APP Callback function
static void app_recv(void)
{
	PROCESS_CONTEXT_BEGIN(&null_app_process);

	uint8_t *data = packetbuf_dataptr();

	rimeaddr_t *sent_sn_addr = packetbuf_addr(PACKETBUF_ADDR_SENDER);

	uint8_t rx_sn_id = sent_sn_addr->u8[0];
	uint8_t pkt_seq = packetbuf_attr(PACKETBUF_ATTR_PACKET_ID);
	uint8_t payload_len = packetbuf_datalen();
/*
	uart1_writeb(rx_sn_id);
	uart1_writeb(pkt_seq);
	uart1_writeb(payload_len);
*/
	if(node_id != 0)
	{
		printf("%u,%u,%u\n",rx_sn_id,pkt_seq,payload_len);
	}
	else
	{
		//printf("%u,%u,%u,",rx_sn_id,pkt_seq,payload_len);
		putchar(rx_sn_id);
		putchar(pkt_seq);
		putchar(payload_len);
		
		//printf("%u",rx_sn_id);
		uint8_t i = 0;
	
		for(i = 0; i < (payload_len); i++)
		{
			//printf("%d,",((data[2*i+1]<<8)|data[2*i]));
			putchar(data[i]);
		}

	
	}

/*
	for(i = 0; i < payload_len; i++)
	{
		uart1_writeb(data[i]);
	}
*/
	uart1_writeb('\n');


	PROCESS_CONTEXT_END(&null_app_process);

}

/*---------------------------------------------------------------------------*/
static void
input_packet(void)
{
  frame802154_t frame;
  int len;

  len = packetbuf_datalen();

  if(frame802154_parse(packetbuf_dataptr(), len, &frame) &&
     packetbuf_hdrreduce(len - frame.payload_len)) {
    if(frame.fcf.dest_addr_mode) {
      if(frame.dest_pid != mac_src_pan_id &&
         frame.dest_pid != FRAME802154_BROADCASTPANDID) {
        /* Not broadcast or for our PAN */
        PRINTF("6MAC: for another pan %u\n", frame.dest_pid);
        return;
      }
      if(!is_broadcast_addr(frame.fcf.dest_addr_mode, frame.dest_addr)) {
        packetbuf_set_addr(PACKETBUF_ADDR_RECEIVER, (rimeaddr_t *)&frame.dest_addr);
        if(!rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
                         &rimeaddr_node_addr)) {
          /* Not for this node */
          PRINTF("6MAC: not for us\n");
          return;
        }
      }
    }
    packetbuf_set_addr(PACKETBUF_ADDR_SENDER, (rimeaddr_t *)&frame.src_addr);

    PRINTF("6MAC-IN: %2X", frame.fcf.frame_type);
    PRINTADDR(packetbuf_addr(PACKETBUF_ADDR_SENDER));
    PRINTADDR(packetbuf_addr(PACKETBUF_ADDR_RECEIVER));
    PRINTF("%u\n", packetbuf_datalen());
    NETSTACK_MAC.input();
  } else {
    PRINTF("6MAC: failed to parse hdr\n");
  }
}

/*---------------------------------------------------------------------------*/
static void
fast_c_packet_sent(int mac_status)
{
  /* Check if our parent just ACKed a DAO */
  if(fast_c_parent_knows_us == 0
     && mac_status == MAC_TX_OK
     && packetbuf_attr(PACKETBUF_ATTR_NETWORK_ID) == UIP_PROTO_ICMP6
     && packetbuf_attr(PACKETBUF_ATTR_CHANNEL) == (ICMP6_RPL << 8 | RPL_CODE_DAO)) {
    if(!linkaddr_cmp(&fast_c_parent_linkaddr, &linkaddr_null)
       && linkaddr_cmp(&fast_c_parent_linkaddr, packetbuf_addr(PACKETBUF_ADDR_RECEIVER))) {
      fast_c_parent_knows_us = 1;
    }
  }
}

/*---------------------------------------------------------------------------*/
static int
create(void)
{
  struct nullmac_hdr *hdr;

  if(packetbuf_hdralloc(sizeof(struct nullmac_hdr))) {
    hdr = packetbuf_hdrptr();
    linkaddr_copy(&(hdr->sender), &linkaddr_node_addr);
    linkaddr_copy(&(hdr->receiver), packetbuf_addr(PACKETBUF_ADDR_RECEIVER));
    return sizeof(struct nullmac_hdr);
  }
  PRINTF("PNULLMAC-UT: too large header: %u\n", sizeof(struct nullmac_hdr));
  return FRAMER_FAILED;
}

/*---------------------------------------------------------------------------*/
static int
read_packet(void)
{
  frame802154_t frame;
  int len;
  packetbuf_clear();
  len = radio->read(packetbuf_dataptr(), PACKETBUF_SIZE);
  if(len > 0) {
    packetbuf_set_datalen(len);
    if(frame802154_parse(packetbuf_dataptr(), len, &frame) &&
       packetbuf_hdrreduce(len - frame.payload_len)) {
      if(frame.fcf.dest_addr_mode) {
        if(frame.dest_pid != mac_src_pan_id &&
           frame.dest_pid != FRAME802154_BROADCASTPANDID) {
          /* Not broadcast or for our PAN */
          PRINTF("6MAC: for another pan %u\n", frame.dest_pid);
          return 0;

        }
        if(!is_broadcast_addr(frame.fcf.dest_addr_mode, frame.dest_addr.u8)) {
          packetbuf_set_addr(PACKETBUF_ADDR_RECEIVER, &frame.dest_addr);
        }
      }
      packetbuf_set_addr(PACKETBUF_ADDR_SENDER, &frame.src_addr);

      PRINTF("6MAC-IN: %2X", frame.fcf.frame_type);
      PRINTADDR(packetbuf_addr(PACKETBUF_ADDR_SENDER));
      PRINTADDR(packetbuf_addr(PACKETBUF_ADDR_RECEIVER));
      PRINTF("%u\n", packetbuf_datalen());
      return packetbuf_datalen();
    } else {
      PRINTF("6MAC: failed to parse hdr\n");
    }
  }
  return 0;
}

void c_multihop_recv(struct pipe *p, struct stackmodule_i *module) {
	PRINTF("c_multihop_recv \n");
	printaddr(module->stack_id);

	rimeaddr_t *nexthop = NULL;

	rimeaddr_t esender, ereceiver;

	/* Copy the packet attributes to avoid them being overwritten or
	 cleared by an application program that uses the packet buffer for
	 its own needs. */
	//rimeaddr_copy(&esender, packetbuf_addr(PACKETBUF_ADDR_ESENDER));
	rimeaddr_copy(&ereceiver, packetbuf_addr(PACKETBUF_ADDR_ERECEIVER));

	if (rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_ERECEIVER),
			&rimeaddr_node_addr)) {
		stack[module->stack_id].resend_flg = 1;
		PRINTF("for us!\n");
		p->multihop_param.hop_no = packetbuf_attr(PACKETBUF_ATTR_HOPS);
	} else {
		nexthop = c_forward(p, stack[module->stack_id].amodule, stack[module->stack_id].modno);
		PRINTF("nexthop: %d.%d \n", nexthop->u8[0], nexthop->u8[1]);

		packetbuf_set_attr(PACKETBUF_ATTR_HOPS,
				packetbuf_attr(PACKETBUF_ATTR_HOPS) + 1);
		if (nexthop) {
			set_node_addr(module->stack_id, 0, 2, nexthop);
			PRINTF("forwarding to %d.%d\n", nexthop->u8[0], nexthop->u8[1]);
			if (module->stack_id < STACKNO) {
				//stack_send(&stack[module->stack_id], module->module_id);
				stack_send(&stack[module->stack_id], module->module_id);

			}
		}
	}
}

/*---------------------------------------------------------------------------*/
void
neighbor_info_packet_received(void)
{
  const rimeaddr_t *src;

  src = packetbuf_addr(PACKETBUF_ADDR_SENDER);
  if(rimeaddr_cmp(src, &rimeaddr_null)) {
    return;
  }

  PRINTF("neighbor-info: packet received from %d.%d\n",
	src->u8[sizeof(*src) - 2], src->u8[sizeof(*src) - 1]);

  add_neighbor(src);
}

/*---------------------------------------------------------------------------*/
void
neighbor_info_packet_sent(int status, int numtx)
{
  const rimeaddr_t *dest;
  link_metric_t packet_metric;
#if UIP_DS6_LL_NUD
  uip_ds6_nbr_t *nbr;
#endif /* UIP_DS6_LL_NUD */

  dest = packetbuf_addr(PACKETBUF_ADDR_RECEIVER);
  if(rimeaddr_cmp(dest, &rimeaddr_null)) {
    return;
  }

  packet_metric = numtx;

  PRINTF("neighbor-info: packet sent to %d.%d, status=%d, metric=%u\n",
	dest->u8[sizeof(*dest) - 2], dest->u8[sizeof(*dest) - 1],
	status, (unsigned)packet_metric);

  switch(status) {
  case MAC_TX_OK:
    add_neighbor(dest);
#if UIP_DS6_LL_NUD
    nbr = uip_ds6_nbr_ll_lookup((uip_lladdr_t *)dest);
    if(nbr != NULL &&
       (nbr->state == STALE || nbr->state == DELAY || nbr->state == PROBE)) {
      nbr->state = REACHABLE;
//      stimer_set(&nbr->reachable, UIP_ND6_REACHABLE_TIME / 1000);
      PRINTF("neighbor-info : received a link layer ACK : ");
      PRINTLLADDR((uip_lladdr_t *)dest);
      PRINTF(" is reachable.\n");
    }
#endif /* UIP_DS6_LL_NUD */
    break;
  case MAC_TX_NOACK:
    add_neighbor(dest);
    printf("neighbor-info: ETX_NOACK_PENALTY\n");
    packet_metric = ETX_NOACK_PENALTY;
    break;
  default:
    /* Do not penalize the ETX when collisions or transmission
       errors occur. */
    return;
  }

  update_metric(dest, packet_metric);
}

//APP Callback function
static void app_recv(void)
{
	PROCESS_CONTEXT_BEGIN(&null_app_process);

#ifdef SF_MOTE_TYPE_AP
	mpu_data_acc_gyro_union data[SAMPLES_PER_FRAME];

	// TX info
	uint8_t i;
	rimeaddr_t *sent_sn_addr = packetbuf_addr(PACKETBUF_ADDR_SENDER);
	uint8_t rx_sn_id = sent_sn_addr->u8[0];

	uint8_t pkt_seq = packetbuf_attr(PACKETBUF_ATTR_PACKET_ID);
	//DATA_SIZE;
	uint8_t payload_len = packetbuf_datalen();

	if(payload_len != MPU_DATA_ACC_GYRO_SIZE*SAMPLES_PER_FRAME)
	{
		//printf("Ignore packet %u,%u %u\n",rx_sn_id,pkt_seq,payload_len);
		//return;
		memcpy((uint8_t*)data,packetbuf_dataptr(),MPU_DATA_ACC_GYRO_SIZE*SAMPLES_PER_FRAME);
	}
	else
	{
		memcpy((uint8_t*)data,packetbuf_dataptr(),MPU_DATA_ACC_GYRO_SIZE*SAMPLES_PER_FRAME);
	}

	for(i=0;i<SAMPLES_PER_FRAME;i++)
	{
		// MPU data
		mpu_data_acc_gyro_union samples = data[i];


		MPU_PRINT_BYTE(rx_sn_id);
		MPU_PRINT_BYTE(0);
		MPU_PRINT_BYTE(pkt_seq);
		MPU_PRINT_BYTE(0);
		print_mpu_sample_acc_gyro(&samples);
		MPU_PRINT_BYTE('\n');
	}

#endif

	PROCESS_CONTEXT_END(&null_app_process);

}

/*---------------------------------------------------------------------------*/
static int
select_packet(uint16_t *slotframe, uint16_t *timeslot)
{
  /* Select data packets we have a unicast link to */
  const linkaddr_t *dest = packetbuf_addr(PACKETBUF_ADDR_RECEIVER);
  if(packetbuf_attr(PACKETBUF_ATTR_FRAME_TYPE) == FRAME802154_DATAFRAME
     && !linkaddr_cmp(dest, &linkaddr_null)) {
    if(slotframe != NULL) {
      *slotframe = slotframe_handle;
    }
    if(timeslot != NULL) {
      *timeslot = get_node_timeslot(dest);
    }
    return 1;
  }
  return 0;
}

/**
 * Input callback called by the lower layers to indicate incoming data
 */
static void dtn_network_input(void) 
{
	linkaddr_t source;
	uint8_t * buffer = NULL;
	uint8_t length = 0;
	packetbuf_attr_t rssi = 0;

	leds_on(LEDS_ALL);

	/* Create a copy here, because otherwise packetbuf_clear will evaporate the address */
	linkaddr_copy(&source, packetbuf_addr(PACKETBUF_ADDR_SENDER));
	buffer = packetbuf_dataptr();
	length = packetbuf_datalen();
	rssi = packetbuf_attr(PACKETBUF_ATTR_RSSI);

	convergence_layer_incoming_frame(&source, buffer, length, rssi);

	leds_off(LEDS_ALL);
}

/* Returns the latestForwarded_table_entry pointer to the latestForwardedTable
*  entry representing the neighbor node address passed as a parameter. If
*  no such neighbor exists, returns NULL.
*/
latestForwarded_table_entry* latestForwardedTableFind(rimeaddr_t *neighbor_p) {
	pmesg(200, "%s :: %s :: Line #%d\n", __FILE__, __func__, __LINE__);
	static uint8_t i = 0;
	static bool isBroadcast = 0;
  
  isBroadcast = rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
                                &rimeaddr_null);
	if (isBroadcast)
		return NULL;

	for (i = 0; i < latestForwardedTableActive; i++) {
		if (rimeaddr_cmp(&(latestForwardedTable[i].neighbor), neighbor_p))
		break;
	}

	if(i == latestForwardedTableActive)
		return NULL;

	return &(latestForwardedTable[i]);
}

/**
 * Send a probe packet.
 */
static void
send_probe(void)
{
  struct lpp_hdr *hdr;
  struct announcement_msg *adata;
  struct announcement *a;

  /* Set up the probe header. */
  packetbuf_clear();
  packetbuf_set_datalen(sizeof(struct lpp_hdr));
  hdr = packetbuf_dataptr();
  hdr->type = TYPE_PROBE;
  rimeaddr_copy(&hdr->sender, &rimeaddr_node_addr);
  rimeaddr_copy(&hdr->receiver, packetbuf_addr(PACKETBUF_ADDR_RECEIVER));


  /* Construct the announcements */
  adata = (struct announcement_msg *)((char *)hdr + sizeof(struct lpp_hdr));
  
  adata->num = 0;
  for(a = announcement_list(); a != NULL; a = a->next) {
    adata->data[adata->num].id = a->id;
    adata->data[adata->num].value = a->value;
    adata->num++;
  }

  packetbuf_set_datalen(sizeof(struct lpp_hdr) +
		      ANNOUNCEMENT_MSG_HEADERLEN +
		      sizeof(struct announcement_data) * adata->num);

  /*  PRINTF("Sending probe\n");*/

  /*  printf("probe\n");*/
  
  /* XXX should first check access to the medium (CCA - Clear Channel
     Assessment) and add LISTEN_TIME to off_time_adjustment if there
     is a packet in the air. */
  radio->send(packetbuf_hdrptr(), packetbuf_totlen());

  compower_accumulate(&compower_idle_activity);
}

/*---------------------------------------------------------------------------*/
int
mac_sequence_is_duplicate(void)
{
    int i;

    /*
     * Check for duplicate packet by comparing the sequence number of the incoming
     * packet with the last few ones we saw.
     */
    for(i = 0; i < MAX_SEQNOS; ++i) {
        if(linkaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_SENDER),
                        &received_seqnos[i].sender)) {
            if(packetbuf_attr(PACKETBUF_ATTR_MAC_SEQNO) == received_seqnos[i].seqno) {
                /* Duplicate packet. */
                return 1;
            }
            break;
        }
    }
    return 0;
}

/*---------------------------------------------------------------------------*/
static int
send_packet(void)
{
  const rimeaddr_t *addr;

  addr = packetbuf_addr(PACKETBUF_ADDR_RECEIVER);
  PRINTF("rime-udp: Sending %d bytes to %d.%d\n", packetbuf_totlen(),
         addr->u8[RIMEADDR_SIZE-2], addr->u8[RIMEADDR_SIZE-1]);

  if(rimeaddr_cmp(&rimeaddr_null, addr)) {
    uip_udp_packet_send(broadcast_conn,
                        packetbuf_hdrptr(), packetbuf_totlen());
  } else {
    uip_ip6addr(&unicast_conn->ripaddr, 0xfe80, 0, 0, 0, 0, 0, 0, 0);
    uip_netif_addr_autoconf_set(&unicast_conn->ripaddr, (uip_lladdr_t *)addr);
    uip_udp_packet_send(unicast_conn,
                        packetbuf_hdrptr(), packetbuf_totlen());
    uip_create_unspecified(&unicast_conn->ripaddr);
  }
  return 1;
}

/*---------------------------------------------------------------------------*/
void
uip_ds6_link_neighbor_callback(int status, int numtx)
{
  const linkaddr_t *dest = packetbuf_addr(PACKETBUF_ADDR_RECEIVER);
  if(linkaddr_cmp(dest, &linkaddr_null)) {
    return;
  }

  LINK_NEIGHBOR_CALLBACK(dest, status, numtx);

#if UIP_DS6_LL_NUD
  /* From RFC4861, page 72, last paragraph of section 7.3.3:
   *
   *         "In some cases, link-specific information may indicate that a path to
   *         a neighbor has failed (e.g., the resetting of a virtual circuit). In
   *         such cases, link-specific information may be used to purge Neighbor
   *         Cache entries before the Neighbor Unreachability Detection would do
   *         so. However, link-specific information MUST NOT be used to confirm
   *         the reachability of a neighbor; such information does not provide
   *         end-to-end confirmation between neighboring IP layers."
   *
   * However, we assume that receiving a link layer ack ensures the delivery
   * of the transmitted packed to the IP stack of the neighbour. This is a
   * fair assumption and allows battery powered nodes save some battery by
   * not re-testing the state of a neighbour periodically if it
   * acknowledges link packets. */
  if(status == MAC_TX_OK) {
    uip_ds6_nbr_t *nbr;
    nbr = uip_ds6_nbr_ll_lookup((uip_lladdr_t *)dest);
    if(nbr != NULL && nbr->state != NBR_INCOMPLETE) {
      nbr->state = NBR_REACHABLE;
      stimer_set(&nbr->reachable, UIP_ND6_REACHABLE_TIME / 1000);
      PRINTF("uip-ds6-neighbor : received a link layer ACK : ");
      PRINTLLADDR((uip_lladdr_t *)dest);
      PRINTF(" is reachable.\n");
    }
  }
#endif /* UIP_DS6_LL_NUD */

}

/*---------------------------------------------------------------------------*/
static void
send_packet(mac_callback_t sent_callback, void *ptr)
{
  const rimeaddr_t *addr;

  addr = packetbuf_addr(PACKETBUF_ADDR_RECEIVER);
  PRINTF("rime-udp: Sending %d bytes to %d.%d\n", packetbuf_totlen(),
         addr->u8[0], addr->u8[1]);

  if(rimeaddr_cmp(&rimeaddr_null, addr)) {
    uip_udp_packet_send(broadcast_conn,
                        packetbuf_hdrptr(), packetbuf_totlen());
    mac_call_sent_callback(sent_callback, ptr, MAC_TX_OK, 1);
  } else {
    uip_ip6addr(&unicast_conn->ripaddr, 0xfe80, 0, 0, 0, 0, 0, 0, 0);
    uip_netif_addr_autoconf_set(&unicast_conn->ripaddr, (uip_lladdr_t *)addr);
    uip_udp_packet_send(unicast_conn,
                        packetbuf_hdrptr(), packetbuf_totlen());
    uip_create_unspecified(&unicast_conn->ripaddr);
  }
  return;
}

/*---------------------------------------------------------------------------*/
void
serial_lprf_mac_rf_input(void)
{
	uint8_t bufCnt, *bufPtr = packetbuf_dataptr();
#if (DEVICE_MODE == SERIAL_LPRF_MAC)
	uint8_t *addrPtr;
#endif
	
	PUTSTRING("Data Received : ");
	PUTSTRING(bufPtr);
	PUTSTRING("\n");
	leds_toggle(LEDS_RED);
#if (DEVICE_MODE == SERIAL_LPRF_MAC)
	putchar(SERIAL_LPRF_MAC_PACKET_START_BYTE);
	putchar(packetbuf_datalen() + 1 + 8);
	putchar('R');
	addrPtr = (uint8_t *)packetbuf_addr(PACKETBUF_ADDR_SENDER);
	for(bufCnt = 0; bufCnt < 8; bufCnt++)
		putchar( * (addrPtr  + bufCnt));
	for(bufCnt = 0; bufCnt < packetbuf_datalen(); bufCnt++)
		putchar( * (bufPtr + bufCnt));
#endif
}