#include <thread>
#include <chrono>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/wait.h>

#include "GeoNetworkingLayer.hh"
#include "GeoNetworkingTypes.hh"

#include "loggers.hh"

using namespace LibItsGeoNetworking__TypesAndValues;

GeoNetworkingLayer::GeoNetworkingLayer(const std::string & p_type, const std::string & param) : TLayer<LibItsGeoNetworking__TestSystem::GeoNetworkingPort>(p_type), _params(), _codec(), _beacon(NULL), _sendData(), _timerid{0}, _sev{0}, _its{0}, _freq_nanosecs(0), _mask{0}, _sa{0} {
  loggers::get_instance().log(">>> GeoNetworkingLayer::GeoNetworkingLayer: %s, %s", to_string().c_str(), param.c_str());
  
  // Setup parameters
  Params::convert(_params, param);
  // Sanity checks
  INTEGER latitude;
  Params::const_iterator it = _params.find(Params::latitude);
  if (it != _params.cend()) {
    latitude = str2int(CHARSTRING(it->second.c_str()));
  }
  INTEGER longitude;
  it = _params.find(Params::longitude);
  if (it != _params.cend()) {
    longitude = str2int(CHARSTRING(it->second.c_str()));
  }
  OCTETSTRING ll_address;
  it = _params.find(Params::ll_address);
  if (it != _params.cend()) {
    ll_address = str2oct(CHARSTRING(it->second.c_str()));
  }
  // Add broadcast address if needed
  it = _params.find("mac_bc");
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("mac_bc"), "FFFFFFFFFFFF"));
  }
  fill_beacon(latitude, longitude, ll_address);
  Params::const_iterator i = _params.find(Params::beaconing);
  if ((i != _params.cend()) && (i->second.compare("1") == 0)) {
    start_beaconing();
  }
} // End of constructor

GeoNetworkingLayer::~GeoNetworkingLayer() {
  if (_timerid != 0) {
    timer_delete(_timerid);
  }	
} // End of destructor

void GeoNetworkingLayer::sendMsg(const LibItsGeoNetworking__TestSystem::GeoNetworkingReq& p, Params& params) {
  loggers::get_instance().log(">>> GeoNetworkingLayer::sendMsg");

  // Encode GeoNetworking PDU
  OCTETSTRING data;
  _codec.encode(p.msgOut(), data);
  sendData(data, params);
}

void GeoNetworkingLayer::sendData(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::sendData: ", data);
  while (_sendData.try_lock() == FALSE) {
    // not ready yet
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  } // End of 'while' statement
  sendToAllLayers(data, params);
  _sendData.unlock();
  loggers::get_instance().log("<<< GeoNetworkingLayer::sendData");
}

void GeoNetworkingLayer::receiveData(OCTETSTRING& data, Params& params) { 
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::receiveData: ", data);
  // Decode the payload
  LibItsGeoNetworking__TestSystem::GeoNetworkingInd p;
  _codec.decode(data, p.msgIn(), &params);
  // Add lower layers parameters
  // 1. Destination MAC address
  Params::const_iterator it = params.find(Params::mac_dst);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: dst=%s", it->second.c_str());
    p.macDestinationAddress() = str2oct(CHARSTRING(it->second.c_str()));
  } else {
    p.macDestinationAddress() = str2oct(CHARSTRING(_params["mac_bc"].c_str()));
  }
  // 2. ssp
  it = params.find(Params::ssp);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: ssp=%s", it->second.c_str());
    p.ssp() = str2bit(CHARSTRING(it->second.c_str()));
  } else {
    p.ssp().set_to_omit();
  }
  // 3. its_aid
  it = params.find(Params::its_aid);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: its_aid=%s", it->second.c_str());
    p.its__aid() = std::stoi(it->second.c_str());
  } else {
    p.its__aid().set_to_omit();
  }
  
  // Pass the GeoNetworking raw payload to the upper layers if any
  it = params.find(Params::gn_payload);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: gn_payload=%s", it->second.c_str());
    OCTETSTRING os(str2oct(CHARSTRING(it->second.c_str())));
    receiveToAllLayers(os, params);
  } else {
    loggers::get_instance().warning("GeoNetworkingLayer::receiveData: No payload to pass to upper layers");
  }
  // Pass it to the ports
  toAllUpperPorts(p, params);
}


void GeoNetworkingLayer::start_beaconing() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::start_beaconing");
  
  // Establish handler for timer signal
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: Establishing handler for signal %d\n", SIGALRM);
  _sa.sa_flags = SA_SIGINFO;
  _sa.sa_sigaction = timer_irq_sigalrm_handler;
  sigemptyset(&_sa.sa_mask);
  if (sigaction(SIGALRM, &_sa, NULL) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigaction failure: %d", errno);
  }
  // Block timer signal temporarily
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: Blocking signal %d\n", SIGALRM);
  sigemptyset(&_mask);
  sigaddset(&_mask, SIGALRM);
  if (sigprocmask(SIG_SETMASK, &_mask, NULL) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigprocmask failure: %d", errno);
  }	
  // Create the timer 
  _sev.sigev_notify = SIGEV_SIGNAL;
  _sev.sigev_signo = SIGALRM; // Use signal alarm
  _sev.sigev_value.sival_ptr = this; // The GeoNetworkingLayer object address
  if (timer_create(CLOCK_REALTIME, &_sev, &_timerid) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Timer failure: %d", errno);
  }
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: timer ID is 0x%lx\n", (long)_timerid);
  // Start the timer
  unsigned int expiry = 1000; // Default expiry time 1000ms
  Params::const_iterator i = _params.find("expiry");
  if (i != _params.cend()) {
    expiry = static_cast<unsigned int>(std::strtoul(i->second.c_str(), NULL, 10));
  }
  _freq_nanosecs = expiry * 1000000;
  _its.it_value.tv_sec = _freq_nanosecs / 1000000000;
  _its.it_value.tv_nsec = _freq_nanosecs % 1000000000;
  _its.it_interval.tv_sec = _its.it_value.tv_sec;
  _its.it_interval.tv_nsec = _its.it_value.tv_nsec;
  if (timer_settime(_timerid, 0, &_its, NULL) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigprocmask failure: %d", errno);
  }
  // Unlock the timer signal, so that timer notification can be delivered
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: Unblocking signal %d\n", SIGALRM);
  if (sigprocmask(SIG_UNBLOCK, &_mask, NULL) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigprocmask failure: %d", errno);
  }
}

void GeoNetworkingLayer::stop_beaconing() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::stop_beaconing");
  
  // Block timer signal temporarily
  loggers::get_instance().log("GeoNetworkingLayer::stop_beaconing: Blocking signal %d\n", SIGALRM);
  sigemptyset(&_mask);
  sigaddset(&_mask, SIGALRM);
  if (sigprocmask(SIG_SETMASK, &_mask, NULL) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::stop_beaconing: Sigprocmask failure: %d", errno);
  }	
  timer_delete(_timerid);
  _timerid = 0;
}

void GeoNetworkingLayer::send_beacon() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::send_beacon");
  
  ExtendedHeader* eh = static_cast<ExtendedHeader *>(_beacon->gnPacket().packet().extendedHeader().get_opt_value());
  if (eh == NULL) {
    loggers::get_instance().error("GeoNetworkingLayer::send_beacon: Wrong cast");
  }
  // Update timestamp
  unsigned long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - 1072911600000L; // TODO Add method such as its_tme() & its_time_mod()
  eh->beaconHeader().srcPosVector().timestamp__() = ms;
  // Encode message using TITAN because of payload in omited
  TTCN_Buffer encoding_buffer;
  _beacon->encode(*(_beacon->get_descriptor()), encoding_buffer, TTCN_EncDec::CT_RAW);
  OCTETSTRING data(encoding_buffer.get_len(), encoding_buffer.get_data());
  // Send it
  Params params(_params);
  sendData(data, params);
  
  loggers::get_instance().log("<<< GeoNetworkingLayer::send_beacon");
} // End of send_beacon method

void GeoNetworkingLayer::fill_beacon(INTEGER& p_latitude, INTEGER& p_longitude, OCTETSTRING& p_ll_address)
{
  _beacon = new GeoNetworkingPdu();
  HeaderTST h;
  h.beaconHdr() = BeaconHeaderType(
				   HeaderType(HeaderType::e__beacon),
				   0
				   );
  ExtendedHeader eh;
  eh.beaconHeader() = BeaconHeader(
				   LongPosVector(
						 GN__Address(
							     TypeOfAddress(TypeOfAddress::e__manual),
							     StationType(StationType::e__roadSideUnit),
							     33,
							     p_ll_address
							     ),
						 0,
						 p_latitude,
						 p_longitude,
						 int2bit(0, 1),
						 0,
						 0
						 )
				   );
  _beacon->basicHeader() = BasicHeader(
				       0,
				       BasicNextHeader(
						       BasicNextHeader::e__commonHeader
						       ),
				       0,
				       Lifetime(
						4,
						LtBase(LtBase::e__50ms)
						),
				       5
				       );
  _beacon->gnPacket().packet() = GnNonSecuredPacket(
						    CommonHeader(
								 NextHeader(
									    NextHeader::e__any
									    ),
								 0,
								 h,
								 TrafficClass(
									      SCF(SCF::e__scfDisabled),
									      ChannelOffload(ChannelOffload::e__choffDisabled),
									      0
									      ),
								 int2bit(0, 8),
								 0,
								 1,
								 0
								 ),
						    OPTIONAL<ExtendedHeader>(eh),
						    OPTIONAL<GnRawPayload>()
						    );
  _beacon->gnPacket().packet().payload().set_to_omit();
  _beacon->gnPacket().securedMsg().set_to_omit();
  //  loggers::get_instance().log_msg("GeoNetworkingLayer::GeoNetworkingLayer: beacon value: ", *p._beacon);
}

  
void GeoNetworkingLayer::timer_irq_sigalrm_handler(int p_signal, siginfo_t *p_signal_info, void *p_uc) {
  loggers::get_instance().log("GeoNetworkingLayer::timer_irq_sigalrm_handler: Caught signal %d\n", p_signal);

  static_cast<GeoNetworkingLayer *>(p_signal_info->si_value.sival_ptr)->send_beacon();
} // End of method timer_irq_sigalrm_handler

class GeoNetworkingFactory: public LayerFactory {
  static GeoNetworkingFactory _f;
public:
  GeoNetworkingFactory();
  virtual Layer * createLayer(const std::string & type,
			      const std::string & param);
};

GeoNetworkingFactory::GeoNetworkingFactory() {
  // Register factory
  loggers::get_instance().log(">>> GeoNetworkingFactory::GeoNetworkingFactory");
  LayerStackBuilder::RegisterLayerFactory("GN", this);
}

Layer * GeoNetworkingFactory::createLayer(const std::string & type, const std::string & param) {
  return new GeoNetworkingLayer(type, param);
}

GeoNetworkingFactory GeoNetworkingFactory::_f;