#include "ipv6.h"

namespace ipv6 {
/******** costants ********/
const IPv6Addr ALL_NODES_ADDRESS("\xff\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01"); /* ff02::1 */

/******** FUNCTIONS *******/
uint16_t compute_checksum(const char* src_addr, const char* dst_addr, uint8_t next_header, const char* payload, int len) {
  char buffer[LEN];
  uint32_t total;
  uint16_t* ptr;
  int words;
  int i = len;

  /* prepare buffer with all data to checksum:
      - the actual payload
      - the pseudo ipv6 header
  */
  /* actual payload */
  memcpy(buffer, payload, len);
  if (len % 2) {  /* pad with 0 if length is odd */
    buffer[len] = 0;
    i++;
  }
  /* pseudo ipv6 header -- see RFC 2460, Section 8.1 */
  memcpy(buffer + i, src_addr, 16); i += 16;
  memcpy(buffer + i, dst_addr, 16); i += 16;
  memset(buffer + i, 0, 3); i+= 3;  /* first 3 bytes always 0, because len always < 256 */
  buffer[i] = len & 0xff; i += 1;
  memset(buffer + i, 0, 3); i += 3; /* zero, as per-standard */
  buffer[i] = next_header; i += 1;

  /* do computation */
  total = 0;
  ptr = (uint16_t*)buffer;
  words = (i + 1) / 2;

  while (words--) total += *ptr++;

  while (total & 0xffff0000) {
    total = (total >> 16) + (total & 0xffff);
  }

  return ~((uint16_t) total);
}

/******** IPv6Addr ********/
IPv6Addr::IPv6Addr() {;}

IPv6Addr::IPv6Addr(const char* address) {
  this->deserialize(address);
}

void IPv6Addr::deserialize(const char* address) {
  memcpy(this->address, address, 16);
}

const char* IPv6Addr::serialize() {
  return this->address;
}

/******** IPv6Packet ********/
char* IPv6Packet::serialize() { /* do we actually need these function? everything is already store in a tokenized/serialized form */
  return packet;
}

void IPv6Packet::deserialize(const char* buffer, int len) {
  memcpy(packet, buffer, len);
}

void IPv6Packet::setNextHeader(uint8_t next_header) {
  *(packet + 6) = next_header;
}

void IPv6Packet::setSrcAddress(IPv6Addr& address) {
  memcpy(packet + 8, (void*)&address, 16);
}
void IPv6Packet::setDstAddress(IPv6Addr& address) {
  memcpy(packet + 24, (void*)&address, 16);
}

void IPv6Packet::setFlow(const char* flow) {
  /* three bytes
    4 most significant bits are ignored
    because only 20 bits can be used */
  *(packet + 1) = (*(packet + 1)) | (flow[0] & 0x0f); /* pay attention to TC field */
  memcpy(packet + 2, flow + 1, 2);
}

void IPv6Packet::setPayload(char* payload, int len) {
    memset(packet, 0, LEN);
    
    *(packet) = 0x60;               /* IP version 6 */
    *(packet + 4 + 1) = len & 0xff; /* length */
    *(packet + 7) = 64;             /* hop limit / ttl */

    memcpy(packet + 40, payload, len);  /* actual payload */
  }

 void IPv6Packet::doAction() {
  /* if destination is my address, or destination is "all nodes" address, then... */
  if ((memcmp(&(ALL_NODES_ADDRESS), packet + 24, 16) == 0) || (memcmp(&(coppino::address), packet + 24, 16) == 0)) {
    int len = ((*(packet + 4)) << 8) + (*(packet + 5));
    
    switch(*(packet + 6)) { /* next header */
      case NH_ICMP:
        handleICMP(packet + 8, packet + 24, packet + 1, packet + 40, len);
        break;
      case NH_UDP:
        // handleUDP(packet + 40, len);
        break;
      default:
        /* no other protocols are known here, sorry :-) */
        break;
    }
  }
 }

void handleICMP(const char* src_addr, const char* dst_addr, const char* flow, char* packet, int len) {
  uint8_t type = *(packet);
  
  if (type == ICMP_ECHO_REQUEST_TYPE) {  /* if it is ICMPv6 Echo Request, then... */
      /* compute checksum, and assert validity of incoming message */
      char received_checksum[2];
      memcpy(received_checksum, packet + 2, 2);

      memset(packet + 2, 0, 2); /* zero-checksum */
      uint16_t computed_checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len);

      //if (memcmp(&computed_checksum, received_checksum, 2) == 0) {  /* if checksum is valid, then... */
        /*  Build an echo reply:
         *  the reply has the same exact format of the request,
         *  except for Type and Checksum */
        *(packet) = ICMP_ECHO_REPLY_TYPE;
        memset(packet + 2, 0, 2); /* zero-checksum */

        uint16_t checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len);
        memcpy(packet + 2, &checksum, 2);
  
        IPv6Packet reply_packet;
        reply_packet.setPayload(packet, len);
        reply_packet.setFlow(flow);
  
        IPv6Addr tmp_addr;
        memcpy(&tmp_addr, dst_addr, 16);
        reply_packet.setSrcAddress(tmp_addr);
        memcpy(&tmp_addr, src_addr, 16);
        reply_packet.setDstAddress(tmp_addr);
        reply_packet.setNextHeader(NH_ICMP);
        slip::send((void*)&reply_packet, 40 + len);
      //}
  }
  return;
}

}