#include <mac.h>
#include <net.h>
#include <routing.h>

#define NUM_HOPS_LIMIT 10

static unsigned char seq_num= 0;

typedef struct {
  net_addr_t    dst;
  unsigned char num_hops;
  //  unsigned char seq_num;
  unsigned long timer;
} rrep_cache_entry_t;

#define RREP_CACHE_SIZE 10
static rrep_cache_entry_t rrep_cache[RREP_CACHE_SIZE];
static unsigned char rrep_cache_size= 0;

static inline void rrep_cache_init() {
  unsigned char i;
  for (i= 0; i < RREP_CACHE_SIZE; i++) {
    rrep_cache[i].dst= NET_ADDR_ANY;
  }
}

static inline void rrep_cache_add(rrep_t * rrep) {
  print("rt:rrep_cache_add\n\r");
  unsigned char i;
  for (i= 0; i < RREP_CACHE_SIZE; i++) {
    if (rrep_cache[i].dst == NET_ADDR_ANY) {
      net_addr_print(rrep->dst);
      print("\n\r");
      rrep_cache[i].dst= rrep->dst;
      rrep_cache[i].num_hops= rrep->num_hops;
      //rrep_cache[i].seq_num= rrep->dst_seq_num;
      rrep_cache[i].timer= millis();
      return;
    }
  }
}

static inline void rreq_cache_add(rreq_t * rreq) {
  print("rt:rreq_cache_add\n\r");
  unsigned char i;
  for (i= 0; i < RREP_CACHE_SIZE; i++) {
    if (rrep_cache[i].dst == NET_ADDR_ANY) {
      net_addr_print(rreq->orig);
      print("\n\r");
      rrep_cache[i].dst= rreq->orig;
      rrep_cache[i].num_hops= rreq->num_hops;
      //rrep_cache[i].seq_num= rreq->orig_seq_num;
      rrep_cache[i].timer= millis();
      return;
    }
  }
}
static inline int rrep_cache_find(net_addr_t dst) {
  unsigned char i;
  for (i= 0; i < RREP_CACHE_SIZE; i++) {
    printdec(i);
    print(":");
    net_addr_print(rrep_cache[i].dst);
    print("=?=");
    net_addr_print(dst);
    print("\n\r");
    if (rrep_cache[i].dst == dst)
      return i;
  }
  return -1;
}

void routing_init() {
  rrep_cache_init();
}

static inline void _send_to_neighbors(const unsigned char * m,
				      unsigned char m_len,
				      saddr_t except) {
  print("_to_nei\n\r");
  unsigned char i;
  for (i= 0; i < neighbors_count(); i++) {
    saddr_t mac_dst= neighbors_get_at(i);
    if (mac_dst == except)
      continue;

    // TO BE IMPLEMENTED: should only propagate to real adjacencies,
    // i.e. neighbors that can see each others

    print("+--to: ");
    printhex(mac_dst);
    while (!mac_can_send()) {
      print(".");
    }
    mac_send(mac_dst, MAC_TYPE_ROUTING, 0, m, m_len);
    print(" *\n\r");
  }
}

/**
 * Send a routing request (RREQ) originated from this node.
 */
void routing_send_rreq(net_addr_t dst) {
  print("rt:tx RREQ\n\r");
  rreq_t rreq;
  rreq.hdr.type= ROUTING_TYPE_RREQ;
  rreq.orig= net_get_local_addr();
  rreq.dst= dst;
  rreq.orig_seq_num= seq_num++;
  rreq.num_hops= 0;
  _send_to_neighbors(&rreq, sizeof(rreq), 0xFFFF);
}

/**
 * Send a routing reply (RREP) originated from this node.
 */
void routing_send_rrep(net_addr_t dst, net_addr_t orig) {
  print("rt:tx RREP\n\r");
  rrep_t rrep;
  rrep.hdr.type= ROUTING_TYPE_RREP;
  rrep.orig= orig;
  rrep.dst= dst;
  rrep.num_hops= 0;
  rrep.dst_seq_num= seq_num++;
  saddr_t mac_dst= net_rt_lookup(orig);
  if (mac_dst == 0xFFFF)
    return;
  while (!mac_can_send()) {
    print(".");
  }
  mac_send(mac_dst, MAC_TYPE_ROUTING, 0, &rrep, sizeof(rrep));
}

void routing_recv(mac_frame * frame) {
  int i;
  switch (frame->payload[1]) {
  case ROUTING_TYPE_RREQ:
    print("rt:rx RREQ\n\r");
    print("+--from:");
    printhex(frame->src.m2.saddr);
    rreq_t * rreq= (rreq_t *) &(frame->payload[1]);
    print("\n\r+--orig:");
    net_addr_print(rreq->orig);
    print("\n\r+--dst :");
    net_addr_print(rreq->dst);
    print("\n\r+--#hop:");
    printdec(rreq->num_hops);
    print("\n\r");

    i= rrep_cache_find(rreq->orig);
    print("i=");
    printdec(i);
    print("\n\r");
    if (i < 0) {
      rreq_cache_add(rreq);
    } else {
      if (rrep_cache[i].num_hops > rreq->num_hops) {
	rrep_cache[i].num_hops= rreq->num_hops;
	rrep_cache[i].timer= millis();
      } else {
	return;
      }
    }

    net_rt_add(rreq->orig, frame->src.m2.saddr);

    // ignore a RREQ looping back to origin
    if (rreq->orig == net_get_local_addr())
      return;

    // RREQ arrived at destination -> originate a RREP
    if (rreq->dst == net_get_local_addr()) {
      routing_send_rrep(rreq->dst, rreq->orig);
    } else {

      // forward to neighbors ?
      if (rreq->num_hops >= NUM_HOPS_LIMIT)
	return;

      // TO BE IMPLEMENTED
      // do not forward if same RREQ received earlier, based on orig's seq#
      // if (rreq->orig_seq_num <= XXX)
      // return;

      // update num_hops
      rreq->num_hops++;

      // forward the RREQ to neighbors
      _send_to_neighbors(rreq, sizeof(rreq_t), frame->src.m2.saddr);
    }
    break;

  case ROUTING_TYPE_RREP:
    print("rt: rx RREP\n\r");
    rrep_t * rrep= (rrep_t *) &(frame->payload[1]);
    print("+--from:");
    printhex(frame->src.m2.saddr);
    print("\n\r+--orig:");
    net_addr_print(rrep->orig);
    print("\n\r+--dst :");
    net_addr_print(rrep->dst);
    print("\n\r+--#hop:");
    printdec(rrep->num_hops);
    //print("\n\r+--seq#:");
    //printdec(rrep->dst_seq_num);
    print("\n\r");

    // TO BE IMPLEMENTED: need to compare to current route
    i= rrep_cache_find(rrep->dst);
    if (i < 0) {
      print("** add\n\r");
      rrep_cache_add(rrep);
    } else {
      /*if (rrep_cache[i].seq_num < rrep->dst_seq_num) {
	print("** replace\n\r");
	rrep_cache[i].seq_num= rrep->dst_seq_num;
	rrep_cache[i].num_hops= rrep->num_hops;
	} else */if (/*(rrep_cache[i].seq_num == rrep->dst_seq_num) &&*/
								    (rrep_cache[i].num_hops > rrep->num_hops)) {
	print("** replace\n\r");
	rrep_cache[i].num_hops= rrep->num_hops;
	rrep_cache[i].timer= millis();
      } else {
	return;
      } 
    }

    // update FIB
    net_rt_add(rrep->dst, frame->src.m2.saddr);

    if (rrep->orig != net_get_local_addr()) {
      // forward the RREP
      saddr_t mac_dst= net_rt_lookup(rrep->orig);
      if (mac_dst == 0xFFFF)
	return;
      rrep->num_hops++;
      while (!mac_can_send()) {
	print(".");
      }
      mac_send(mac_dst, MAC_TYPE_ROUTING, 0, rrep, sizeof(rrep_t));
    }
    break;
  }
}

void routing_refresh() {
  unsigned char i;
  for (i= 0; i < RREP_CACHE_SIZE; i++) {
    if (rrep_cache[i].dst != NET_ADDR_ANY) {
      if (rrep_cache[i].timer < millis()) {
	print("rt:remove ");
	net_addr_print(rrep_cache[i].dst);
	print("\n\r");
	rrep_cache[i].dst= NET_ADDR_ANY;
      }
    }
  }
}
