#include "PacketDissector.h"
#include "Logger/Logger.h"
#include <iostream>
#include <sstream>

unsigned char * strIntToBin(const string& sInt) {
  int * pBin = new int(0);
  istringstream is(sInt);

  if(sInt.substr(0, 2) == "0x")
    is >> hex;

  if(!(is >> *pBin)) {
    stringstream ss;
    ss << "Bad filter: not an integer " << sInt;
    Logger::Instance().LogError(ss.str());
  }
  return reinterpret_cast<unsigned char *>(pBin);
}

unsigned char * strIpToBin(const string& sIp) {

  unsigned char * pBin = new unsigned char[4];
  int previousPos = 0;
  int pos;
  
  for(int i=0; i < 4; i++) {
    pos = sIp.find(".", previousPos);
    if(pos == string::npos && i != 3) {
      stringstream ss;
      ss << "Bad filter: invalid IP address " << sIp;
      Logger::Instance().LogError(ss.str());
      return pBin;
    }
    pBin[i] = *strIntToBin(sIp.substr(previousPos, pos - previousPos));
    previousPos = pos + 1;
  }

  return pBin;
}

PacketDissector::PacketDissector(const char* psPacket) 
  :m_ethHdr(0),
   m_ipHdr(0),
   m_ipv6Hdr(0),
   m_tcpHdr(0),
   m_udpHdr(0),
   m_payload(0) {

  m_ethHdr = new EthernetHeader;
  memcpy(m_ethHdr, psPacket, sizeof(EthernetHeader));
  psPacket += sizeof(EthernetHeader);
  
  switch (m_ethHdr->type) {
  case ETH_TYPE_IPv6:
    m_ipv6Hdr = new IpV6Header;
    memcpy(m_ipv6Hdr, psPacket, sizeof(IpV6Header));
    psPacket += sizeof(IpV6Header);
    
    switch (m_ipv6Hdr->tos) {
	    case IP_PROTO_TCP:
	      m_tcpHdr = new TcpHeader;
	      memcpy(m_tcpHdr, psPacket, sizeof(TcpHeader));
	      psPacket += sizeof(TcpHeader);
	      memcpy(m_payload, psPacket, (m_tcpHdr->dataOffset - 5) * 4);
	      break;
	    case IP_PROTO_UDP:
	      m_udpHdr = new UdpHeader;
	      memcpy(m_udpHdr, psPacket, sizeof(UdpHeader));
	      psPacket += sizeof(UdpHeader);
	      memcpy(m_payload, psPacket, m_udpHdr->length - 8);
	      break;
	    default:
	      Logger::Instance().LogError("Bad filter (ipv6): protocol not supported");
	      break;
	    }
    break;

  case ETH_TYPE_IP:
    m_ipHdr = new IpHeader;
    memcpy(m_ipHdr, psPacket, sizeof(IpHeader));
    psPacket += sizeof(IpHeader);

    switch (m_ipHdr->protocol) {
    case IP_PROTO_TCP:
      m_tcpHdr = new TcpHeader;
      memcpy(m_tcpHdr, psPacket, sizeof(TcpHeader));
      psPacket += sizeof(TcpHeader);
      memcpy(m_payload, psPacket, (m_tcpHdr->dataOffset - 5) * 4);
      break;
    case IP_PROTO_UDP:
      m_udpHdr = new UdpHeader;
      memcpy(m_udpHdr, psPacket, sizeof(UdpHeader));
      psPacket += sizeof(UdpHeader);
      memcpy(m_payload, psPacket, m_udpHdr->length - 8);
      break;
    default:
      Logger::Instance().LogError("Bad filter: protocol not supported");
      break;
    }
    break;
  default:
    Logger::Instance().LogError("Bad filter: message type not supported");
    break;
  }
}

PacketDissector::~PacketDissector() {

  delete m_ethHdr;
  delete m_ipv6Hdr;
  delete m_ipHdr;
  delete m_tcpHdr;
  delete m_udpHdr;
  delete m_payload;
}

bool PacketDissector::Match(const HeaderId& headerId, const FieldId& fieldId, const string& sValue) const {
  if(m_ipHdr && headerId == e_Ip) {
    switch(fieldId) {
    case e_Addr:
      return (!memcmp(&(m_ipHdr->srcAddr), strIpToBin(sValue), 4) 
	      || !memcmp(&(m_ipHdr->destAddr), strIpToBin(sValue), 4));
    case e_Src:
      return !memcmp(&(m_ipHdr->srcAddr), strIpToBin(sValue), 4);
    case e_Dst:
      return !memcmp(&(m_ipHdr->destAddr), strIpToBin(sValue), 4);
    case e_Proto:
      return !memcmp(&(m_ipHdr->protocol), strIntToBin(sValue), 1);
    default:
      Logger::Instance().LogError("Bad filter: invalid field for IPv4 header");
      return false;      
    }
  } else if(m_ipv6Hdr && headerId == e_Ip) {
    switch(fieldId) {
    case e_Addr:
      return (!memcmp(&(m_ipv6Hdr->srcAddr), strIpToBin(sValue), 16) 
	      || !memcmp(&(m_ipv6Hdr->destAddr), strIpToBin(sValue), 16));
    case e_Src:
      return !memcmp(&(m_ipv6Hdr->srcAddr), strIpToBin(sValue), 16);
    case e_Dst:
      return !memcmp(&(m_ipv6Hdr->destAddr), strIpToBin(sValue), 16);
    case e_Proto:
      return !memcmp(&(m_ipv6Hdr->tos), strIntToBin(sValue), 1);
    default:
      Logger::Instance().LogError("Bad filter: invalid field for IPv6 header");
      return false;      
    }
  }
  
  if(m_tcpHdr && headerId == e_Tcp) {
    switch(fieldId) {
    case e_Port:
      return (!memcmp(&(m_tcpHdr->srcPort), strIntToBin(sValue), 2)
	      || !memcmp(&(m_tcpHdr->destPort), strIntToBin(sValue), 2));
    case e_SrcPort:
      return !memcmp(&(m_tcpHdr->srcPort), strIntToBin(sValue), 2);
    case e_DstPort:
      return !memcmp(&(m_tcpHdr->destPort), strIntToBin(sValue), 2);
    default:
      Logger::Instance().LogError("Bad filter: invalid field for TCP header");
      return false;
    }
  }

  if(m_udpHdr && headerId == e_Udp) {
    switch(fieldId) {
    case e_Port:
      return (!memcmp(&(m_udpHdr->srcPort), strIntToBin(sValue), 2)
	      || !memcmp(&(m_udpHdr->destPort), strIntToBin(sValue), 2));
    case e_SrcPort:
      return !memcmp(&(m_udpHdr->srcPort), strIntToBin(sValue), 2);
    case e_DstPort:
      return !memcmp(&(m_udpHdr->destPort), strIntToBin(sValue), 2);
    default:
      Logger::Instance().LogError("Bad filter: invalid field for UPD header");
      return false;
    }
  }

  Logger::Instance().LogError("Bad filter: no support for this header type");
  return false;
}
