#include <mac.h>
#include <neighbors.h>
#include <print.h>
#include <rom_cfg.h>

#define NDEBUG

#ifdef NDEBUG
#define PRINT(M) do {} while (0)
#define PRINTHEX(M) do {} while (0)
#define PRINTDEC(M) do {} while (0)
#else
#define PRINT(M) print(M)
#define PRINTHEX(M) printhex(M)
#define PRINTDEC(M) printdec(M)
#endif

static neighbor_t neighbors[MAX_NEIGHBORS];
static unsigned char num_neighbors= 0;

unsigned char neighbors_count() {
  return num_neighbors;
}

void neighbors_init() {
  unsigned char i;
  for (i= 0; i < MAX_NEIGHBORS; i++) {
    neighbors[i].addr= 0xFFFF;
    neighbors[i].seen= 0;
  }
  num_neighbors= 0;
}

void neighbors_add(unsigned short addr, char seen,
		   unsigned char rssi, unsigned char lqi) {
  unsigned char free= MAX_NEIGHBORS;
  unsigned char i;
  for (i= 0; i < MAX_NEIGHBORS; i++) {
    if (neighbors[i].addr == addr) {
      free= i;
      num_neighbors--;
      break;
    } else if (neighbors[i].addr == 0xFFFF) {
      if (free == MAX_NEIGHBORS)
	free= i;
    }
  }
  if (free == MAX_NEIGHBORS) {
    PRINT("Error: too many neighbors\n\r");
    return; // Too many neighbors
  }
  neighbors[free].addr= addr;
  neighbors[free].seen= seen;
  neighbors[free].rssi= rssi;
  neighbors[free].lqi= lqi;
  neighbors[free].timer= 5;
  num_neighbors+= 1;
  PRINT("** added/updated\n\r");
}

void neighbors_refresh() {
  PRINT("nei:refresh\n\r");
  unsigned char i;
  for (i= 0; i < MAX_NEIGHBORS; i++) {
    if (neighbors[i].addr != 0xFFFF) {
      if (neighbors[i].timer == 0) {
	PRINT("+-- addr: ");
	PRINTDEC(neighbors[i].addr);
	PRINT(" REM\n\r");
	neighbors[i].addr= 0xFFFF;
	num_neighbors--;
      } else {
	neighbors[i].timer--;
      }
    }
  }
}

void neighbors_dump() {
  int i;
  for (i= 0; i < MAX_NEIGHBORS; i++) {
    if (neighbors[i].addr != 0xFFFF) {
      print("+-- addr: ");
      printhex(neighbors[i].addr);
      print(" seen:");
      printdec(neighbors[i].seen);
      print(" rssi:");
      printdec(neighbors[i].rssi);
      print(" lqi:");
      printdec(neighbors[i].lqi);
      print(" timer:");
      printdec(neighbors[i].timer);
      print("\n\r");
    }
  }
}

static unsigned char payload[1 + (MAX_NEIGHBORS * 4)];

void neighbors_send_advertisement() {
  unsigned char i;
  unsigned char * payload_ptr= payload;
  *(payload_ptr++)= num_neighbors;
  for (i= 0; i < MAX_NEIGHBORS; i++) {
    if (neighbors[i].addr != 0xFFFF) {
      *(payload_ptr++)= (neighbors[i].addr & 0xFF);
      *(payload_ptr++)= (neighbors[i].addr >> 8);
      *(payload_ptr++)= neighbors[i].rssi;
      *(payload_ptr++)= neighbors[i].lqi;
    }
  }
  mac_send(0xFFFF, 0, MAC_TYPE_NEIGHBOR_ADVERTISEMENT,
	   payload, payload_ptr-payload);
}

saddr_t neighbors_get_at(unsigned char i) {
  unsigned char j= 0;
  while ((j < MAX_NEIGHBORS) && (i >= 0)) {
    if (neighbors[j].addr != 0xFFFF) {
      if (i == 0)
	return neighbors[j].addr;
      i--;
    }
    j++;
  }
  return 0xFFFF;
}

void neighbors_recv_advertisement(mac_frame * frame, unsigned char rssi,
				  unsigned char lqi) {
  PRINT("nei:rx response\n\r");
  PRINT("+--from:");
  if (frame->src.mode != 0x02)
    return;
  PRINTHEX(frame->src.m2.saddr);
  PRINT("\n\r+--cnt :");
  PRINTDEC(frame->payload[1]);
  PRINT("\n\r");
  unsigned char i;
  unsigned short nei_addr;
  unsigned char nei_rssi;
  unsigned char nei_lqi;
  char seen= 0;
  unsigned char * payload_ptr= &frame->payload[2];
  for (i= 0; i < frame->payload[1]; i++) {
    nei_addr= *(payload_ptr) + (*(payload_ptr+1) << 8);
    payload_ptr+= 2;
    nei_rssi= *(payload_ptr++);
    nei_lqi= *(payload_ptr++);
    PRINT("+--addr: ");
    PRINTHEX(nei_addr);
    PRINT(" rssi: ");
    PRINTDEC(nei_rssi);
    PRINT(" lqi: ");
    PRINTDEC(nei_lqi);
    PRINT("\n\r");
    if (nei_addr == eeprom_config.saddr)
      seen= 1;
  }
  neighbors_add(frame->src.m2.saddr, seen, rssi, lqi);
}

