#include "coppino.h"

namespace coppino {
  ipv6::IPv6Addr address;

  void handler() {
    
    char buffer[LEN];
    int r;
    
    while ((r = slip::recv(buffer, LEN)) != -1) {
      ipv6::IPv6Packet packet;
      packet.deserialize(buffer, r);
      packet.doAction();
    }
    
  }

  void hexdump(const char* pointer, int len) {
    for (int i = 0; i < len; ++i) {
      Serial.print(pointer[i] & 0xff, HEX);
      Serial.print('.');
    }
  }

  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;

    memcpy(buffer, payload, len);
    if (len % 2) {
      buffer[len] = 0;
      i++;
    }
    memcpy(buffer + i, src_addr, 16); i += 16;
    memcpy(buffer + i, dst_addr, 16); i += 16;
    memcpy(buffer + i, "\0\0", 2); i += 2;
    buffer[i] = len >> 8; i += 1;
    buffer[i] = len & 0xff; i += 1;
    memcpy(buffer + i, "\0\0\0", 3); i += 3;
    buffer[i] = next_header; i += 1;

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

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

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

    //total -= 64*256; // TODO -- this is an ugly fix, something is wrong somewhere else

    return ~((uint16_t) total);
    /*
    uint16_t checksum = 0;
      for (i = 0; i < len; i = i + 2) {
        part = ((*(packet + i + 0)) << 8) + *(packet + i + 1);
        checksum += part;
      }
      for (i = 0; i < 16; i = i + 2 ) {
        part = ((*(src_addr + i + 0)) << 8) + *(src_addr + i + 1);
        checksum += part;
        part = ((*(dst_addr + i + 0)) << 8) + *(dst_addr + i + 1);
        checksum += part;
      }
      checksum += (len + 58);  // upper-layer packet length + next header
      while (checksum >> 16)
        checksum = (checksum & 0xffff) + (checksum >> 16);
    */  
  }

  void blink() {
    digitalWrite(13, !digitalRead(13));
  }
}