/*---------------------------------------------------------------------------*/
static void
packet_input(void)
{
  if(NETSTACK_FRAMER.parse() < 0) {
    PRINTF("br-rdc: failed to parse %u\n", packetbuf_datalen());
  } else {
    NETSTACK_MAC.input();
  }
}

/*---------------------------------------------------------------------------*/
static void packet_input(void) {
    // printf("WPI-MAC-packet_input(), node ID: %u\n", node_id);
    if(NETSTACK_FRAMER.parse() < 0) {
        printf("WPI-MAC: failed to parse %u\n", packetbuf_datalen());
    } else {
        NETSTACK_MAC.input();
        // printf("%s\n", "PASSED UP\n");
    }
}

/*---------------------------------------------------------------------------*/
static void
broadcast_recv(struct broadcast_conn *c, const linkaddr_t *from)
{
  printf("*** Received %u bytes from %u:%u: '0x%04u' ", packetbuf_datalen(),
         from->u8[0], from->u8[1], *(uint16_t *)packetbuf_dataptr());
  printf("%d - %u\n", (int8_t)packetbuf_attr(PACKETBUF_ATTR_RSSI),
         packetbuf_attr(PACKETBUF_ATTR_LINK_QUALITY));
  leds_toggle(LEDS_GREEN);
}

/*---------------------------------------------------------------------------*/
static void
rrep_packet_received(struct unicast_conn *uc, const rimeaddr_t *from)
{
    struct rrep_hdr *msg = packetbuf_dataptr();
    struct route_entry *rt;
    rimeaddr_t dest;
    struct route_discovery_conn *c = (struct route_discovery_conn *)
                                     ((char *)uc - offsetof(struct route_discovery_conn, rrepconn));

    PRINTF("%d.%d: rrep_packet_received from %d.%d towards %d.%d len %d\n",
           rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
           from->u8[0],from->u8[1],
           msg->dest.u8[0],msg->dest.u8[1],
           packetbuf_datalen());

    PRINTF("from %d.%d hops %d rssi %d lqi %d\n",
           from->u8[0], from->u8[1],
           msg->hops,
           packetbuf_attr(PACKETBUF_ATTR_RSSI),
           packetbuf_attr(PACKETBUF_ATTR_LINK_QUALITY));

    insert_route(&msg->originator, from, msg->hops);

    if(rimeaddr_cmp(&msg->dest, &rimeaddr_node_addr))
    {
        PRINTF("rrep for us!\n");
        rrep_pending = 0;
        ctimer_stop(&c->t);
        if(c->cb->new_route)
        {
            rimeaddr_t originator;

            /* If the callback modifies the packet, the originator address
               will be lost. Therefore, we need to copy it into a local
               variable before calling the callback. */
            rimeaddr_copy(&originator, &msg->originator);
            c->cb->new_route(c, &originator);
        }

    }
    else
    {
        rimeaddr_copy(&dest, &msg->dest);

        rt = route_lookup(&msg->dest);
        if(rt != NULL)
        {
            PRINTF("forwarding to %d.%d\n", rt->nexthop.u8[0], rt->nexthop.u8[1]);
            msg->hops++;
            unicast_send(&c->rrepconn, &rt->nexthop);
        }
        else
        {
            PRINTF("%d.%d: no route to %d.%d\n", rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1], msg->dest.u8[0], msg->dest.u8[1]);
        }
    }
}

/*---------------------------------------------------------------------------*/
static void
recv(const linkaddr_t *originator, uint8_t seqno, uint8_t hops)
{
  printf("Sink got message from %d.%d, seqno %d, hops %d: len %d '%s'\n",
         originator->u8[0], originator->u8[1],
         seqno, hops,
         packetbuf_datalen(),
         (char *)packetbuf_dataptr());
}

/*---------------------------------------------------------------------------*/
static void
packet_input(void)
{
#if NULLRDC_802154_AUTOACK
  if(packetbuf_datalen() == ACK_LEN) {
    /* Ignore ack packets */
    /* PRINTF("nullrdc: ignored ack\n"); */
  } else
#endif /* NULLRDC_802154_AUTOACK */
  if(NETSTACK_FRAMER.parse() < 0) {
    PRINTF("nullrdc: failed to parse %u\n", packetbuf_datalen());
#if NULLRDC_ADDRESS_FILTER
  } else if(!rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
                                         &rimeaddr_node_addr) &&
            !rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
                          &rimeaddr_null)) {
    PRINTF("nullrdc: not for us\n");
#endif /* NULLRDC_ADDRESS_FILTER */
  } else {
#if NULLRDC_802154_AUTOACK || NULLRDC_802154_AUTOACK_HW
    /* Check for duplicate packet by comparing the sequence number
       of the incoming packet with the last few ones we saw. */
    int i;
    for(i = 0; i < MAX_SEQNOS; ++i) {
      if(packetbuf_attr(PACKETBUF_ATTR_PACKET_ID) == received_seqnos[i].seqno &&
         rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_SENDER),
                      &received_seqnos[i].sender)) {
        /* Drop the packet. */
        PRINTF("nullrdc: drop duplicate link layer packet %u\n",
               packetbuf_attr(PACKETBUF_ATTR_PACKET_ID));
        return;
      }
    }
    for(i = MAX_SEQNOS - 1; i > 0; --i) {
      memcpy(&received_seqnos[i], &received_seqnos[i - 1],
             sizeof(struct seqno));
    }
    received_seqnos[0].seqno = packetbuf_attr(PACKETBUF_ATTR_PACKET_ID);
    rimeaddr_copy(&received_seqnos[0].sender,
                  packetbuf_addr(PACKETBUF_ADDR_SENDER));
#endif /* NULLRDC_802154_AUTOACK */
    NETSTACK_MAC.input();
  }
}

static void
recv(struct mesh_conn *c, const rimeaddr_t *from, uint8_t hops)
{
  printf("Data received from %d.%d: %s (%d)\n",
         from->u8[0], from->u8[1],
         (char *)packetbuf_dataptr(), packetbuf_datalen());

//  packetbuf_copyfrom("Hop", strlen("Hop"));
//  mesh_send(&mesh, from);
}

/* This can only be called with packet-buf filled with a packet and
   it's attributes */
int
transmit_buffer_add_packet(clock_time_t time, uint8_t id)
{
  int i;
  for(i = 0; i < MAX_TX_BUF; i++) {
    if(buffers[i].len == 0) {
      /* copy the packet data into the buffer */
      /* what more needs to be stored? */
      buffers[i].len = packetbuf_datalen();
      buffers[i].id = id;
      memcpy(buffers[i].packet, packetbuf_dataptr(), packetbuf_datalen());
      buffers[i].time = time;
      buffers[i].txmits = packetbuf_attr(PACKETBUF_ATTR_MAX_MAC_TRANSMISSIONS);
      update_timer();
      return 1;
    }
  }
  return 0;
}

/*---------------------------------------------------------------------------*/
void
sniffer_input()
{
  uint8_t *pkt; 
  uint16_t pkt_len;
  uint8_t rssi;
  uint8_t lqi;
  uint16_t timestamp;
  uint16_t i;

  pkt = packetbuf_dataptr();
  pkt_len = packetbuf_datalen();
  rssi = packetbuf_attr(PACKETBUF_ATTR_RSSI);
  lqi = packetbuf_attr(PACKETBUF_ATTR_LINK_QUALITY);
  timestamp = packetbuf_attr(PACKETBUF_ATTR_TIMESTAMP);
  /*
  printf("New packet\n");
  printf("Pakcet len: %u\n", pkt_len);
  printf("Packet:");
  for (i = 0; i < pkt_len; i++) {
    printf(" %2x", pkt[i]);
  }
  printf("\n");
  printf("CRC: none\n");
  printf("CRC OK: %d\n", !!sniffer_crc_ok);
  printf("RSSI: %u\n", 255 - rssi);
  printf("LQI: %u\n", lqi);
  printf("Timestamp: %u\n", timestamp);
  */
  /* magic | type | len | pkt | crc_ok | rssi | lqi */
  for (i = 0; i < MAGIC_LEN; i++) {
    putchar(magic[i]);
  }
  putchar(MY_TYPE);
  putchar((uint8_t) pkt_len);
  for (i = 0; i < pkt_len; i++) {
    putchar(pkt[i]);
  }
//  if (MY_TYPE & FIELD_CRC) {
//    putchar(sniffer_crc[0]);
//    putchar(sniffer_crc[1]);
//  }
  if (MY_TYPE & FIELD_CRC_OK) {
    putchar(sniffer_crc_ok);
  }
  if (MY_TYPE & FIELD_RSSI) {
    putchar(rssi);
  }
  if (MY_TYPE & FIELD_LQI) {
    putchar(lqi);
  }
  if (MY_TYPE & FIELD_TIMESTAMP) {
    putchar((timestamp >> 8) & 0xFF);
    putchar(timestamp & 0xFF);
  }

/*---------------------------------------------------------------------------*/
static int
slip_radio_cmd_handler(const uint8_t *data, int len)
{
  int i;
  if(data[0] == '!') {
    /* should send out stuff to the radio - ignore it as IP */
    /* --- s e n d --- */
    if(data[1] == 'S') {
      int pos;
      packet_ids[packet_pos] = data[2];

      packetbuf_clear();
      pos = packetutils_deserialize_atts(&data[3], len - 3);
      if(pos < 0) {
        PRINTF("slip-radio: illegal packet attributes\n");
        return 1;
      }
      pos += 3;
      len -= pos;
      if(len > PACKETBUF_SIZE) {
        len = PACKETBUF_SIZE;
      }
      memcpy(packetbuf_dataptr(), &data[pos], len);
      packetbuf_set_datalen(len);

      PRINTF("slip-radio: sending %u (%d bytes)\n",
             data[2], packetbuf_datalen());

      /* parse frame before sending to get addresses, etc. */
      no_framer.parse();
      NETSTACK_LLSEC.send(packet_sent, &packet_ids[packet_pos]);

      packet_pos++;
      if(packet_pos >= sizeof(packet_ids)) {
	packet_pos = 0;
      }

      return 1;
    }
  } else if(uip_buf[0] == '?') {
    PRINTF("Got request message of type %c\n", uip_buf[1]);
    if(data[1] == 'M') {
      /* this is just a test so far... just to see if it works */
      uip_buf[0] = '!';
      uip_buf[1] = 'M';
      for(i = 0; i < 8; i++) {
        uip_buf[2 + i] = uip_lladdr.addr[i];
      }
      uip_len = 10;
      cmd_send(uip_buf, uip_len);
      return 1;
    }
  }
  return 0;
}

/*---------------------------------------------------------------------------*/
static void
packet_input(void)
{
#if NETSTACK_CONF_WITH_IP64
  /* If NAT64 is enabled, IPv4 & ARP packets are sent to the IP64 module */
  ethernet_header_t *hdr = (ethernet_header_t *)packetbuf_hdrptr();
  if (hdr->et_protlen == UIP_HTONS(ETH_PROT_IP) ||
    hdr->et_protlen == UIP_HTONS(ETH_PROT_ARP)) {
    IP64_INPUT(packetbuf_hdrptr(), packetbuf_totlen());
    return;
  }
#endif /* NETSTACK_CONF_WITH_IP64 */
  if(NETSTACK_0_FRAMER.parse() < 0) {
    PRINTF("ieee8023_mac: failed to parse %u\n", (unsigned int)packetbuf_datalen()); 
#if IEEE8023_MAC_ADDRESS_FILTER
  } else if(!linkaddr6_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
      &linkaddr6_node_addr) &&
      !linkaddr6_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
      &linkaddr6_null)) {
    /*\note The framer should have done the multicast address
     * translation from 0x33 0x33 0xXX... to the 802.3xx null
     */     
    PRINTF("ieee8023_mac: not for us\n");
#endif /* IEEE8023_MAC_ADDRESS_FILTER */                
  } else {
    int duplicate = 0;
    duplicate = mac_sequence_is_duplicate();

    if(duplicate && ETH_SUPPORT_DUPLICATE_DETECTION) {
      /* Drop the packet. */
      PRINTF("ieee8023_mac: drop duplicate link layer packet %u\n",
      packetbuf_attr(PACKETBUF_ATTR_PACKET_ID));
    } else {
      mac_sequence_register_seqno();
    }
#if DEBUG
    int i;
    uint16_t original_datalen = packetbuf_totlen();
    uint8_t *original_dataptr = packetbuf_hdrptr();
    PRINTF("PKT[%u]:",original_datalen);
    for (i=0; i<original_datalen; i++) {
      PRINTF("%02x ",original_dataptr[i]);
    }
    PRINTF("\n");
#endif

    if(!(duplicate && ETH_SUPPORT_DUPLICATE_DETECTION)) {
      link_if_in(NETSTACK_8023);
      NETSTACK_0_NETWORK.input();
      link_if_in(NETSTACK_NULL);
    } else {
      PRINTF("IEEE8023_MAC: drop duplicate\n");
    }
  }       
}

/*---------------------------------------------------------------------------*/
void 
receive(const rimeaddr_t * from)
{

  message_header_t * msg_header = packetbuf_dataptr();
  PRINTF("[DEBUG] Qprocessor! message received from %d.%d, datalen %d, type %d\n", 
         from->u8[0], from->u8[1], packetbuf_datalen(), msg_header->type);
 
  switch(msg_header->type) {
    case MSG_QREQUEST :
      parse_select_query((qmessage_header_t *)(msg_header+ 1));
      break;
    case MSG_QREPLY :
      PRINTF("[DEBUG] Qprocessor! reply received from %d.%d datalen %d\n",
             from->u8[0], from->u8[1], packetbuf_datalen());
      packetizer_send(packetbuf_dataptr(), packetbuf_datalen());
      break;
  }

}

/*---------------------------------------------------------------------------*/
static void
recv(struct runicast_conn *ruc, const rimeaddr_t *from, uint8_t seqno)
{
  struct rucb_conn *c = (struct rucb_conn *)ruc;

  PRINTF("%d.%d: rucb: recv from %d.%d len %d\n",
	 rimeaddr_node_addr.u8[0],rimeaddr_node_addr.u8[1],
	 from->u8[0], from->u8[1], packetbuf_totlen());

  if(seqno == c->last_seqno) {
    return;
  }
  c->last_seqno = seqno;

  if(rimeaddr_cmp(&c->sender, &rimeaddr_null)) {
    rimeaddr_copy(&c->sender, from);
    c->u->write_chunk(c, 0, RUCB_FLAG_NEWFILE, packetbuf_dataptr(), 0);
    c->chunk = 0;
  }


  if(rimeaddr_cmp(&c->sender, from)) {
    int datalen = packetbuf_datalen();

    if(datalen < RUCB_DATASIZE) {
      PRINTF("%d.%d: get %d bytes, file complete\n",
	     rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
	     datalen);
      c->u->write_chunk(c, c->chunk * RUCB_DATASIZE,
			 RUCB_FLAG_LASTCHUNK, packetbuf_dataptr(), datalen);
    } else {
      c->u->write_chunk(c, c->chunk * RUCB_DATASIZE,
			RUCB_FLAG_NONE, packetbuf_dataptr(), datalen);
    }
    c->chunk++;
  }

  if(packetbuf_datalen() < RUCB_DATASIZE) {
    rimeaddr_copy(&c->sender, &rimeaddr_null);
  }
}

/*---------------------------------------------------------------------------*/
int p_broadcast_send(struct p_broadcast_conn *c, const void *data, size_t length)
{
	PRINTF("[pbroadcast.c] Sending message of size %d\n", (int)length);

	if (length > MAX_BROADCAST_PAYLOAD_SIZE)
	{
		PRINTF("[pbroadcast.c] ^ Error: Broadcast packet is too long.\n");
		return 0;
	}

	if (c == NULL)
	{
		PRINTF("[pbroadcast.c] ^ Error: Broadcast Connection is NULL\n");
		return 0;
	}

	packetbuf_clear();
	packetbuf_set_addr(PACKETBUF_ADDR_SENDER, &linkaddr_node_addr);
	packetbuf_set_datalen(length + sizeof(uint16_t));

	// copy payload into packet
	memcpy(packetbuf_dataptr(), data, length);

	// copy hash to end of packet
	uint16_t hash = packet_hash(data, length);
	memcpy(packetbuf_dataptr() + (uintptr_t)length, &hash, sizeof(uint16_t));

	PRINTF("[pbroadcast.c] ^ Hash: %02X Datalen: %d\n", hash, packetbuf_datalen());
	PRINTF("[pbroadcast.c] ^ Data: ");
	DEBUG_DUMP_DATA(packetbuf_dataptr(), packetbuf_datalen());
	PRINTF("\n");

	int status = abc_send(&(c->abc));

	if (status == 0)
	{
		PRINTF("[pbroadcast.c] Broadcast could not be sent.\n");
	}

	return status;
}

/*---------------------------------------------------------------------------*/
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("\n\n!Incoming Data %u!\n\n", packetbuf_datalen());
    
    
    
    NETSTACK_MAC.input();
  } else {
    PRINTF("6MAC: failed to parse hdr\n");
  }
}

/*---------------------------------------------------------------------------*/
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);
          if(!rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_RECEIVER),
                           &rimeaddr_node_addr)) {
            /* Not for this node */
            PRINTF("6MAC: not for us\n");
            return 0;
          }
        }
      }
      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_abc_recv(struct pipe *p, struct stackmodule_i *module) {
    int start_tm = START_TM;
    rimeaddr_t tmpaddr;

    rimeaddr_copy(&tmpaddr, packetbuf_addr(PACKETBUF_ADDR_SENDER));
    set_node_addr(module->stack_id, 1, 0, &tmpaddr);
    PRINTF("~c_abc_receive: packet %s (%d) on channel %d from %d.%d\n",
           (char *)packetbuf_dataptr(),
           packetbuf_datalen(), p->channel->channelno,
           tmpaddr.u8[0], tmpaddr.u8[1]);
    PRINTFE("\n %d \n", DURATION_TM(start_tm));
}

/*---------------------------------------------------------------------------*/
static void
recv(struct abc_conn *abc)
{
  struct polite_conn *c = (struct polite_conn *)abc;
  if(c->q != NULL &&
     packetbuf_datalen() == queuebuf_datalen(c->q) &&
     memcmp(packetbuf_dataptr(), queuebuf_dataptr(c->q),
	    MIN(c->hdrsize, packetbuf_datalen())) == 0) {
    /* We received a copy of our own packet, so we do not send out
       packet. */
    queuebuf_free(c->q);
    c->q = NULL;
    ctimer_stop(&c->t);
    if(c->cb->dropped) {
      c->cb->dropped(c);
    }
  }
  if(c->cb->recv) {
    c->cb->recv(c);
  }
}

//#define DISPATCH_BYTE_POS 0
//#define 802154_CMDFRM_ID_POS 0
static void
lowpan_packet_input(void)
{
    //uint8_t * rime_ptr = NULL;
    frame802154_t frame;
    // TODO: BSKR is this buflen and not len??
    frame802154_parse(packetbuf_dataptr(), packetbuf_datalen(), &frame);
#ifdef LOWPAN_COORDINATOR
    mac_proc_pkt(&frame);
#else
    mac_proc_state(&frame);
#endif
}

static inline bool handle_ack_packet(struct net_buf *buf)
{
	if (packetbuf_datalen(buf) == ACK_LEN) {
		ack_received = true;
		nano_sem_give(&ack_lock);
#ifdef SIMPLERDC_802154_AUTOACK
		PRINTF("simplerdc: ignore ACK packet\n");
		return true;
#endif
	}

	return false;
}