#include "xbee_wrapper.h"


XBeeWrapper::XBeeWrapper(Node &_node){ 
  xbee = XBee();
  node = _node;
};


void XBeeWrapper::begin(const unsigned long baud){
	xbee.setSerial(Serial1);
	Serial1.begin(baud);
};


uint8_t XBeeWrapper::receive(Packet& pkt){
  xbee.readPacket();

  if( !xbee.getResponse().isAvailable() )
    return XBWRECV_NONE;

  uint8_t apiId = xbee.getResponse().getApiId();

  uint8_t packetRssi = 0;
  uint64_t packetMsb = 0;
  uint64_t packetLsb = 0;

  if( apiId != RX_16_RESPONSE && apiId != RX_64_RESPONSE )
    return XBWRECV_NONE;

  if( apiId == RX_16_RESPONSE ){

    Rx16Response res = Rx16Response();
    xbee.getResponse().getRx16Response(res);

    packetRssi = res.getRssi();
    msb = res.getRemoteAddress16();

    // extract data and propagate error
    if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
      return XBWRECV_ERROR;

  } else {

    Rx64Response res = Rx64Response();
    xbee.getResponse().getRx64Response(res);

    packetRssi = res.getRssi();
    packetMsb = res.getRemoteAddress64().getMsb();
    packetLsb = res.getRemoteAddress64().getLsb();

    // extract data and propagate error
    if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
      return XBWRECV_ERROR;

  }

  if(pkt.getOpcode() != OPCODE_DISCOVER)
    return XBWRECV_OK;

  //if the wave id changed, or if the last rssi is higher than the new one change the rssi
  if(node.update(pkt.getDist(),pkt.getWave()) || (rssi > packetRssi && pkt.getDist() < node.getDist())){
    rssi = packetRssi;
    addrType = apiId;
    msb = packetMsb;
    lsb = packetLsb;
  }

  return XBWRECV_OK;
};

uint8_t XBeeWrapper::broadcast(Packet& pkt){
	// build broadcast addr
	XBeeAddress64 bcast = XBeeAddress64(0x00000000, 0x0000FFFF);

	// build payload from packet
	uint8_t payload[PROTO_SIZE];
  memset(payload,0,PROTO_SIZE);
	size_t payload_size = pkt.write(payload);

	// send
	Tx64Request tx = Tx64Request(bcast, payload, payload_size);
	xbee.send(tx);

	return XBWSEND_OK;
};

uint32_t XBeeWrapper::sendToNearest(Packet& pkt){
  if(addrType != RX_64_RESPONSE && addrType != RX_16_RESPONSE )
    return XBWSEND_NONEAREST;

  // build payload from packet
  uint8_t payload[PROTO_SIZE];
  memset(payload,0,PROTO_SIZE);
  size_t payload_size = pkt.write(payload);

  XBeeRequest* tx;
  if(addrType == RX_64_RESPONSE){
    // build broadcast addr
    XBeeAddress64 addr = XBeeAddress64(msb, lsb);
    
    // instanciate
    tx = new Tx64Request(addr, payload, payload_size);   
  }else{
    // instanciate
    tx = new Tx16Request((uint16_t) msb, payload, payload_size);
  }

  //send
  xbee.send(*tx);

  return XBWSEND_OK;
};