#include "ethernet-header2.h"

#include "ns3/abort.h"
#include "ns3/assert.h"
#include "ns3/header.h"
#include "ns3/log.h"
#include "ns3/mac48-address.h"

#include <iomanip>
#include <iostream>



#include "ns3/address-utils.h"


namespace ns3
{

NS_LOG_COMPONENT_DEFINE("EthernetHeader2");

NS_OBJECT_ENSURE_REGISTERED(EthernetHeader2);

EthernetHeader2::EthernetHeader2()
{
  NS_LOG_FUNCTION_NOARGS();
}

EthernetHeader2::~EthernetHeader2()
{
  NS_LOG_FUNCTION_NOARGS();
}

TypeId
EthernetHeader2::GetTypeId()
{
  NS_LOG_FUNCTION_NOARGS();
  static TypeId tid = TypeId("ns3::EthernetHeader2")
                          .SetParent<Header>()
                          .SetGroupName("Ethernet")
                          .AddConstructor<EthernetHeader2>();
  return tid;
}

TypeId
EthernetHeader2::GetInstanceTypeId() const
{
  NS_LOG_FUNCTION_NOARGS();
  return GetTypeId();
}

void
EthernetHeader2::Print(std::ostream& os) const
{
  NS_LOG_FUNCTION(this);
  std::string proto;

  os << m_src << "->" << m_dest << " with ";
  for(int i=0; i<(int)m_QTagList.size(); i++)
  {
    if(m_QTagList[i].Type == VLAN)
    {
      os <<"vlan_id=" << (m_QTagList[i].TCI & 0xFFF) << " ";
    }
    if(m_QTagList[i].Type == REDUNDANCY)
    {
      os <<"RTAG=True ";
    }
  }
}

uint32_t
EthernetHeader2::GetSerializedSize() const
{
  NS_LOG_FUNCTION_NOARGS();
  int tagSize = 0;
  for (int i=0; i<(int)m_QTagList.size(); i++)
  {
    if(m_QTagList[i].Type==VLAN)
    {
      tagSize += 4;
    }
    else if(m_QTagList[i].Type==REDUNDANCY)
    {
      tagSize += 6;
    }
  }
  return 6 + 6 + tagSize + 2;
}

void
EthernetHeader2::Serialize(Buffer::Iterator start) const
{
  NS_LOG_FUNCTION(this);
  Buffer::Iterator i = start;
  WriteTo(i, m_dest);
  WriteTo(i, m_src);

  for (int j=0; j<(int)m_QTagList.size() ; j++)
  {
    i.WriteHtonU16(m_QTagList[j].TPID);
    if(m_QTagList[j].Type == VLAN)
    {
        i.WriteHtonU16(m_QTagList[j].TCI);
    }
    else if(m_QTagList[j].Type == REDUNDANCY)
    {
      i.WriteHtonU32(m_QTagList[j].TCI);
    }
  }
  i.WriteHtonU16(m_ethertype);
}

uint32_t
EthernetHeader2::Deserialize(Buffer::Iterator start)
{
  NS_LOG_FUNCTION(this);

  Buffer::Iterator i = start;
  ReadFrom(i, m_dest);
  ReadFrom(i, m_src);
  uint16_t tmp = i.ReadNtohU16();
  while (tmp==0x8100 or tmp==0xf1c1){
    TagType type = VLAN;
    uint32_t tci = 0;

    if (tmp==0x8100){
      type = VLAN;
      tci = i.ReadNtohU16();
    }
    else if (tmp==0xf1c1){
      type = REDUNDANCY;
      tci = i.ReadNtohU32();
    }
    Qtag entry = {tmp, tci, type};
    m_QTagList.insert(m_QTagList.end(), entry);
    tmp = i.ReadNtohU16();
  }
  m_ethertype = tmp;

  // NS_LOG_INFO("m_QTagList size = " << m_QTagList.size());
  return GetSerializedSize();
}


void
EthernetHeader2::SetDest(Mac48Address addr)
{
  NS_LOG_FUNCTION(this);
  m_dest = addr;
}

Mac48Address
EthernetHeader2::GetDest()
{
  NS_LOG_FUNCTION(this);
  return m_dest;
}

void
EthernetHeader2::SetSrc(Mac48Address addr)
{
  NS_LOG_FUNCTION(this);
  m_src = addr;
}

Mac48Address
EthernetHeader2::GetSrc()
{
  NS_LOG_FUNCTION(this);
  return m_src;
}


uint8_t
EthernetHeader2::GetPcp()
{
  NS_LOG_FUNCTION(this);
  for(int i=0; i<(int)m_QTagList.size(); i++)
  {
    if(m_QTagList[i].Type == VLAN)
    {
      return (m_QTagList[i].TCI >> 13) & 0x7;
    }
  }
  NS_ASSERT_MSG(true, "Can't find a Vlan QTag in this header");
  return 0;
}

uint8_t
EthernetHeader2::GetDei()
{
  NS_LOG_FUNCTION(this);
  for(int i=0; i<(int)m_QTagList.size(); i++)
  {
    if(m_QTagList[i].Type == VLAN)
    {
      return uint8_t((m_QTagList[i].TCI >> 12) & 0x1);
    }
  }
  NS_ASSERT_MSG(true, "Can't find a Vlan QTag in this header");
  return 0;
}

uint16_t
EthernetHeader2::GetVid()
{
  NS_LOG_FUNCTION(this);
  for(int i=0; i<(int)m_QTagList.size(); i++)
  {
    if(m_QTagList[i].Type == VLAN)
    {
      return (m_QTagList[i].TCI & 0xFFF);
    }
  }
  NS_ASSERT_MSG(true, "Can't find a Vlan QTag in this header");
  return 0;
}

void
EthernetHeader2::SetEthertype(uint16_t ethertype)
{
  NS_LOG_FUNCTION(this);
  m_ethertype = ethertype;
}

uint16_t
EthernetHeader2::GetEthertype()
{
  NS_LOG_FUNCTION(this);
  return m_ethertype;
}

void
EthernetHeader2::SetVlanTag(uint8_t pcp, uint8_t dei, uint16_t vid)
{
  NS_LOG_FUNCTION(this);
  NS_ASSERT_MSG(vid < 4096 , "Vlan id must be between 0 and 4095");
  NS_ASSERT_MSG(dei < 2 , "DEI must be between 0 and 1");
  NS_ASSERT_MSG(pcp < 8 , "PCP must be between 0 and 7");

  uint16_t tpid = 0x8100;
  uint32_t tci = (pcp << 13) | (dei << 12) | vid;
  Qtag entry = {tpid, tci, VLAN};
  m_QTagList.insert(m_QTagList.begin(), entry);
}

void
EthernetHeader2::SetRedundancyTag(uint16_t seqNum)
{
  NS_LOG_FUNCTION(this);
  uint16_t tpid = 0xf1c1;
  uint32_t tci = (0 << 16) | seqNum;
  Qtag entry = {tpid, tci, REDUNDANCY};
  m_QTagList.insert(m_QTagList.begin(), entry);
}

uint16_t
EthernetHeader2::RemoveRedundancyTag()
{
  NS_LOG_FUNCTION(this);
  for (int i=0; i<(int)m_QTagList.size(); i++)
  {
    if(m_QTagList[i].Type == REDUNDANCY)
    {
      uint16_t seqNum = m_QTagList[i].TCI & 0xFFFF;
      m_QTagList.erase(m_QTagList.begin() + i);
      return seqNum;
    }
  }

  NS_ASSERT_MSG(true, "Can't find a Redundancy QTag in this header");
  return 0;

}

}