//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see http://www.gnu.org/licenses/.
// 

#include "GptpTSN.h"

using namespace inet;

Define_Module(GptpTSN);

simsignal_t GptpTSN::localTimeSignal = cComponent::registerSignal("localTime");
simsignal_t GptpTSN::timeDifferenceSignal = cComponent::registerSignal("timeDifference");
simsignal_t GptpTSN::rateRatioSignal = cComponent::registerSignal("rateRatio");
simsignal_t GptpTSN::peerDelaySignal = cComponent::registerSignal("peerDelay");
simsignal_t GptpTSN::correctionFieldSignal = cComponent::registerSignal("correctionField");

const MacAddress GptpTSN::GPTP_MULTICAST_ADDRESS("01:80:C2:00:00:0E");


GptpTSN::~GptpTSN()
{
    cancelAndDeleteClockEvent(selfMsgDelayReq);
    cancelAndDeleteClockEvent(selfMsgSync);
    cancelAndDeleteClockEvent(requestMsg);
}

void GptpTSN::initialize(int stage)
{
    ClockUserModuleBase::initialize(stage);

    if (stage == INITSTAGE_LOCAL) {
        gptpNodeType = static_cast<GptpNodeType>(cEnum::get("GptpNodeType", "inet")->resolve(par("gptpNodeType")));
        domainNumber = par("domainNumber");

        std::hash<std::string> strHash;
        clockIdentity = strHash(getFullPath());
    }

    if (stage == INITSTAGE_LINK_LAYER) {
            peerDelay = 0;
            gmRateRatio = 1.0;
            correctionField = par("correctionField");
            receivedTimeSync = CLOCKTIME_ZERO;

            pDelayReqProcessingTime = par("pDelayReqProcessingTime");

            interfaceTable.reference(this, "interfaceTableModule", true);

            const char *str = par("slavePort");
            if (*str) { // Possui slavePorts, se possui slvaePorts n pode ser um MASTER_NODE
                if (gptpNodeType == MASTER_NODE)
                    throw cRuntimeError("Parameter inconsistency: MASTER_NODE with slave port");
                auto nic = CHK(interfaceTable->findInterfaceByName(str)); // faz com que as slavePorts saibam qnd o pacote entrou no nó
                slavePortId = nic->getInterfaceId();
                nic->subscribe(transmissionEndedSignal, this);
                nic->subscribe(receptionEndedSignal, this);
            }
            else // n possui slavePort, logo deve ser um MASTER_NODE
                if (gptpNodeType != MASTER_NODE)
                    throw cRuntimeError("Parameter error: Missing slave port for %s", par("gptpNodeType").stringValue());

            auto v = check_and_cast<cValueArray *>(par("masterPorts").objectValue())->asStringVector(); // verifica as masterPorts, e se tem ele nao pode ser um SLAVE_NODE
            if (v.empty() and gptpNodeType != SLAVE_NODE)
                throw cRuntimeError("Parameter error: Missing any master port for %s", par("gptpNodeType").stringValue());
            for (const auto& p : v) { // faz com que as masterPorts saibam qnd o pacote entrou no nó
                auto nic = CHK(interfaceTable->findInterfaceByName(p.c_str()));
                int portId = nic->getInterfaceId();
                if (portId == slavePortId) // verifica se a porta n esta setada para ser slave E master
                    throw cRuntimeError("Parameter error: the port '%s' specified both master and slave port", p.c_str());
                masterPortIds.insert(portId);
                nic->subscribe(transmissionEndedSignal, this);
                nic->subscribe(receptionEndedSignal, this);
            }

            if (slavePortId != -1) { // se n é 1 quer dizer q tem slavePort e seta ela como tal
                auto networkInterface = interfaceTable->getInterfaceById(slavePortId);
                if (!networkInterface->matchesMulticastMacAddress(GPTP_MULTICAST_ADDRESS))
                    networkInterface->addMulticastMacAddress(GPTP_MULTICAST_ADDRESS);
            }
            for (auto id: masterPortIds) { // para cada masterPort ela é setada o id
                auto networkInterface = interfaceTable->getInterfaceById(id);
                if (!networkInterface->matchesMulticastMacAddress(GPTP_MULTICAST_ADDRESS))
                    networkInterface->addMulticastMacAddress(GPTP_MULTICAST_ADDRESS);
            }

            registerProtocol(Protocol::gptp, gate("socketOut"), gate("socketIn")); // registra o protocolo (no message dispatcher?)

            /* Only grandmaster in the domain can initialize the synchronization message periodically
             * so below condition checks whether it is grandmaster and then schedule first sync message */
            if(gptpNodeType == MASTER_NODE) // schedules para o GM
            {
                syncInterval = par("syncInterval");
                // Schedule Sync message to be sent
                selfMsgSync = new ClockEvent("selfMsgSync", GPTP_SELF_MSG_SYNC);

                clocktime_t scheduleSync = par("syncInitialOffset");
                originTimestamp = clock->getClockTime() + scheduleSync;
                scheduleClockEventAfter(scheduleSync, selfMsgSync);
            }
            if(slavePortId != -1) // quem n é GM tem q fazer a requisição para calcular peerDelay
            {
                requestMsg = new ClockEvent("requestToSendSync", GPTP_REQUEST_TO_SEND_SYNC);

                // Schedule Pdelay_Req message is sent by slave port
                // without depending on node type which is grandmaster or bridge
                selfMsgDelayReq = new ClockEvent("selfMsgPdelay", GPTP_SELF_MSG_PDELAY_REQ);
                pdelayInterval = par("pdelayInterval");
                scheduleClockEventAfter(par("pdelayInitialOffset"), selfMsgDelayReq);
            }
            WATCH(peerDelay);
        }
}


void GptpTSN::handleMessage(cMessage *msg)
{
    if (msg->isSelfMessage()) {
        handleSelfMessage(msg);
    }
    else {
        Packet *packet = check_and_cast<Packet *>(msg);
        auto gptp = packet->peekAtFront<GptpBase>();
        auto gptpMessageType = gptp->getMessageType();
        auto incomingNicId = packet->getTag<InterfaceInd>()->getInterfaceId();
        int incomingDomainNumber = gptp->getDomainNumber();

        if (incomingDomainNumber != domainNumber) {
            EV_ERROR << "Message " << msg->getClassAndFullName() << " arrived with foreign domainNumber " << incomingDomainNumber << ", dropped\n";
            PacketDropDetails details;
            details.setReason(NOT_ADDRESSED_TO_US);
            emit(packetDroppedSignal, packet, &details);
        }
        else if (incomingNicId == slavePortId) {
            // slave port
            switch (gptpMessageType) {
                case GPTPTYPE_PDELAY_RESP:
                    processPdelayResp(packet, check_and_cast<const GptpPdelayResp *>(gptp.get()));
                    break;
                case GPTPTYPE_PDELAY_RESP_FOLLOW_UP:
                    processPdelayRespFollowUp(packet, check_and_cast<const GptpPdelayRespFollowUp *>(gptp.get()));
                    break;
                case GPTPTYPE_SYNC:
                    processSync(packet, check_and_cast<const GptpSync *>(gptp.get()));
                    break;
                case GPTPTYPE_FOLLOW_UP:
                    processFollowUp(packet, check_and_cast<const GptpFollowUp *>(gptp.get()));
                    // Send a request to send Sync message
                    // through other gptp Ethernet interfaces
                    if(gptpNodeType == BRIDGE_NODE)
                        sendSync();
                    break;
                default:
                    throw cRuntimeError("Unknown gPTP packet type: %d", (int)(gptpMessageType));
            }
        }
        else if (masterPortIds.find(incomingNicId) != masterPortIds.end()) {
            // master port
            if(gptpMessageType == GPTPTYPE_PDELAY_REQ) {
                processPdelayReq(packet, check_and_cast<const GptpPdelayReq *>(gptp.get()));
            }
            else {
                throw cRuntimeError("Unaccepted gPTP type: %d", (int)(gptpMessageType));
            }
        }
        else {
            // passive port
            EV_ERROR << "Message " << msg->getClassAndFullName() << " arrived on passive port " << incomingNicId << ", dropped\n";
        }
        delete msg;
    }
}


void GptpTSN::handleSelfMessage(cMessage *msg)
{
    switch(msg->getKind()) {
        case GPTP_SELF_MSG_PDELAY_REQ:
            ASSERT(slavePortId != -1);
            sendPdelayReq();
            scheduleClockEventAfter(pdelayInterval, selfMsgDelayReq);
            break;
        case GPTP_SELF_REQ_ANSWER_KIND:
            sendPdelayResp(check_and_cast<GptpReqAnswerEvent*>(msg));
            delete msg;
            break;
        case GPTP_SELF_MSG_SYNC:
            sendSync();
            scheduleClockEventAfter(syncInterval, selfMsgSync);
            break;
        default:
            throw cRuntimeError("Unknown self message (%s)%s, kind=%d", msg->getClassName(), msg->getName(), msg->getKind());
    }
}

void GptpTSN::sendPdelayReq()
{
    auto packet = new Packet("GptpPdelayReq");
    auto gptp = makeShared<GptpPdelayReq>();

    PortIdentity portId;
    portId.clockIdentity = clockIdentity;
    portId.portNumber = slavePortId;

    lastSentPdelayReqSequenceId = sequenceId++;

    gptp->setDomainNumber(domainNumber);
    gptp->setCorrectionField(correctionField);
    gptp->setSourcePortIdentity(portId);
    gptp->setSequenceId(lastSentPdelayReqSequenceId);

    packet->addTag<MacAddressReq>()->setDestAddress(GPTP_MULTICAST_ADDRESS);
    packet->insertAtFront(gptp);

    pdelayReqEventEgressTimestamp = clock->getClockTime();
    rcvdPdelayResp = false;

    sendPacketToNIC(packet, slavePortId);
}

void GptpTSN::sendPdelayResp(GptpReqAnswerEvent* req)
{
    int portId = req->getPortId();
    auto gptp = makeShared<GptpPdelayResp>();
    auto packet = new Packet("GptpPdelayResp");

    gptp->setDomainNumber(domainNumber);
    gptp->setRequestingPortIdentity(req->getSourcePortIdentity());
    gptp->setSequenceId(req->getSequenceId());
    gptp->setRequestReceiptTimestamp(req->getIngressTimestamp());

    packet->addTag<MacAddressReq>()->setDestAddress(GPTP_MULTICAST_ADDRESS);
    packet->insertAtFront(gptp);

    sendPacketToNIC(packet, portId);
}

void GptpTSN::sendPdelayRespFollowUp(int portId, const GptpPdelayResp* resp)
{
    auto packet = new Packet("GptpPdelayRespFollowUp");
    auto gptp = makeShared<GptpPdelayRespFollowUp>();

    gptp->setDomainNumber(domainNumber);
    gptp->setResponseOriginTimestamp(clock->getClockTime());
    gptp->setRequestingPortIdentity(resp->getRequestingPortIdentity());
    gptp->setSequenceId(resp->getSequenceId());

    packet->addTag<MacAddressReq>()->setDestAddress(GPTP_MULTICAST_ADDRESS);
    packet->insertAtFront(gptp);

    sendPacketToNIC(packet, portId);
}


void GptpTSN::sendSync()
{
    auto packet = new Packet("GptpSync");
    auto gptp = makeShared<GptpSync>();
    /* OriginTimestamp always get Sync departure time from grand master */
    if (gptpNodeType == MASTER_NODE) {
        originTimestamp = clock->getClockTime();
    }
    //gptp->setOriginTimestamp(CLOCKTIME_ZERO);
    gptp->setDomainNumber(domainNumber);
    gptp->setSequenceId(sequenceId++);

    packet->addTag<MacAddressReq>()->setDestAddress(GPTP_MULTICAST_ADDRESS);
    packet->insertAtFront(gptp);

    for (auto port: masterPortIds)
        sendPacketToNIC(packet->dup(), port);
    delete packet;

    // The sendFollowUp(portId) called by receiveSignal(), when GptpSync sent
}


void GptpTSN::sendFollowUp(int portId, const GptpSync *sync, clocktime_t preciseOriginTimestamp)
{
    auto packet = new Packet("GptpFollowUp");
    auto gptp = makeShared<GptpFollowUp>();

    gptp->setDomainNumber(domainNumber);
    gptp->setPreciseOriginTimestamp(preciseOriginTimestamp); // set o tempo q sync egressou
    gptp->setSequenceId(sync->getSequenceId());
    if (gptpNodeType == MASTER_NODE){
        gptp->setCorrectionField(CLOCKTIME_ZERO);
        emit(localTimeSignal, CLOCKTIME_AS_SIMTIME(clock->getClockTime()));
    }
    else if (gptpNodeType == BRIDGE_NODE)
    {
        gptp->setCorrectionField(correctionField+peerDelay+ oldLocalTimeAtTimeSync - receivedTimeSync);  // TODO revise it!!! see prev. comment, where is the (1-f),  ???
    }
    gptp->getFollowUpInformationTLVForUpdate().setRateRatio(gmRateRatio);

    packet->addTag<MacAddressReq>()->setDestAddress(GPTP_MULTICAST_ADDRESS);
    packet->insertAtFront(gptp);

    sendPacketToNIC(packet, portId);
}

void GptpTSN::sendPacketToNIC(Packet *packet, int portId)
{
    auto networkInterface = interfaceTable->getInterfaceById(portId);
    EV_INFO << "Sending " << packet << " to output interface = " << networkInterface->getInterfaceName() << ".\n";
    packet->addTag<InterfaceReq>()->setInterfaceId(portId);
    packet->addTag<PacketProtocolTag>()->setProtocol(&Protocol::gptp);
    packet->addTag<DispatchProtocolInd>()->setProtocol(&Protocol::gptp);
    auto protocol = networkInterface->getProtocol();
    if (protocol != nullptr)
        packet->addTagIfAbsent<DispatchProtocolReq>()->setProtocol(protocol);
    else
        packet->removeTagIfPresent<DispatchProtocolReq>();
    send(packet, "socketOut");
}

void GptpTSN::processPdelayReq(Packet *packet, const GptpPdelayReq* gptp)
{
    auto resp = new GptpReqAnswerEvent("selfMsgPdelayResp", GPTP_SELF_REQ_ANSWER_KIND);
    resp->setPortId(packet->getTag<InterfaceInd>()->getInterfaceId());
    resp->setIngressTimestamp(packet->getTag<GptpIngressTimeInd>()->getArrivalClockTime());
    resp->setSourcePortIdentity(gptp->getSourcePortIdentity());
    resp->setSequenceId(gptp->getSequenceId());

    scheduleClockEventAfter(pDelayReqProcessingTime, resp);
}

void GptpTSN::processPdelayResp(Packet *packet, const GptpPdelayResp* gptp)
{
    if (gptp->getRequestingPortIdentity().clockIdentity != clockIdentity || gptp->getRequestingPortIdentity().portNumber != slavePortId) {
        EV_WARN << "GptpPdelayResp arrived with invalid PortIdentity, dropped";
        return;
    }
    if (gptp->getSequenceId() != lastSentPdelayReqSequenceId) {
        EV_WARN << "GptpPdelayResp arrived with invalid sequence ID, dropped";
        return;
    }

    rcvdPdelayResp = true;
    pdelayRespEventIngressTimestamp = packet->getTag<GptpIngressTimeInd>()->getArrivalClockTime(); //t4
    peerRequestReceiptTimestamp = gptp->getRequestReceiptTimestamp(); // t2
}


void GptpTSN::processPdelayRespFollowUp(Packet *packet, const GptpPdelayRespFollowUp* gptp)
{
    if (!rcvdPdelayResp) {
        EV_WARN << "GptpPdelayRespFollowUp arrived without GptpPdelayResp, dropped";
        return;
    }
    if (gptp->getRequestingPortIdentity().clockIdentity != clockIdentity || gptp->getRequestingPortIdentity().portNumber != slavePortId) {
        EV_WARN << "GptpPdelayRespFollowUp arrived with invalid PortIdentity, dropped";
        return;
    }
    if (gptp->getSequenceId() != lastSentPdelayReqSequenceId) {
        EV_WARN << "GptpPdelayRespFollowUp arrived with invalid sequence ID, dropped";
        return;
    }

    peerResponseOriginTimestamp = gptp->getResponseOriginTimestamp(); // t3

    peerDelay = (gmRateRatio * (pdelayRespEventIngressTimestamp - pdelayReqEventEgressTimestamp) - (peerResponseOriginTimestamp - peerRequestReceiptTimestamp)) / 2.0;

    EV_INFO << "RATE RATIO                       - " << gmRateRatio << endl;
    EV_INFO << "pdelayReqEventEgressTimestamp    - " << pdelayReqEventEgressTimestamp << endl;
    EV_INFO << "peerRequestReceiptTimestamp      - " << peerRequestReceiptTimestamp << endl;
    EV_INFO << "peerResponseOriginTimestamp      - " << peerResponseOriginTimestamp << endl;
    EV_INFO << "pdelayRespEventIngressTimestamp  - " << pdelayRespEventIngressTimestamp << endl;
    EV_INFO << "PEER DELAY                       - " << peerDelay << endl;

    emit(peerDelaySignal, CLOCKTIME_AS_SIMTIME(peerDelay));
}

void GptpTSN::processSync(Packet *packet, const GptpSync* gptp)
{
    rcvdGptpSync = true;
    lastReceivedGptpSyncSequenceId = gptp->getSequenceId();
    syncIngressTimestamp = packet->getTag<GptpIngressTimeInd>()->getArrivalClockTime(); // chegou sync message
}

void GptpTSN::processFollowUp(Packet *packet, const GptpFollowUp* gptp)
{
    if (!rcvdGptpSync) {
        EV_WARN << "GptpFollowUp arrived without GptpSync, dropped";
        return;
    }
    if (gptp->getSequenceId() != lastReceivedGptpSyncSequenceId) {
        EV_WARN << "GptpFollowUp arrived with invalid sequence ID, dropped";
        return;
    }

    correctionField = gptp->getCorrectionField();
    peerSentTimeSync = gptp->getPreciseOriginTimestamp(); // tempo q sync egressou
    receivedRateRatio = gptp->getFollowUpInformationTLV().getRateRatio();
    synchronize();

    EV_INFO << "############## FOLLOW_UP ################################"<< endl;
    EV_INFO << "RECEIVED TIME AFTER SYNC - " << newLocalTimeAtTimeSync << endl;
    EV_INFO << "ORIGIN TIME SYNC         - " << originTimestamp << endl;
    EV_INFO << "CORRECTION FIELD         - " << correctionField << endl;
    EV_INFO << "PROPAGATION DELAY        - " << peerDelay << endl;

    rcvdGptpSync = false;
}

void GptpTSN::synchronize()
{
    simtime_t now = simTime();
    clocktime_t origNow = clock->getClockTime();
    clocktime_t residenceTime = origNow - syncIngressTimestamp; // o residence time é o tempo entre a chegada de sync e o processamento de follow_up

    emit(timeDifferenceSignal, CLOCKTIME_AS_SIMTIME(origNow) - now);

    /************** Time synchronization *****************************************
     * Local time is adjusted using peer delay, correction field, residence time *
     * and packet transmission time based departure time of Sync message from GM *
     *****************************************************************************/
    clocktime_t newTime = peerSentTimeSync + peerDelay + correctionField + residenceTime;

    ASSERT(gptpNodeType != MASTER_NODE);

    // TODO validate the following expression with the standard
    if (oldPeerSentTimeSync == -1)
        gmRateRatio = 1;
    else{
        gmRateRatio = (peerSentTimeSync - oldPeerSentTimeSync) / (origNow - newLocalTimeAtTimeSync- correctionField);
    }
    auto settableClock = check_and_cast<SettableClock *>(clock.get());
    ppm newOscillatorCompensation = unit(gmRateRatio * (1 + unit(settableClock->getOscillatorCompensation()).get()) - 1);
    settableClock->setClockTime(newTime, newOscillatorCompensation, true);

    oldPeerSentTimeSync = peerSentTimeSync;
    oldLocalTimeAtTimeSync = origNow;
    newLocalTimeAtTimeSync = clock->getClockTime();
    receivedTimeSync = syncIngressTimestamp;

    // adjust local timestamps, too
    pdelayRespEventIngressTimestamp += newLocalTimeAtTimeSync - oldLocalTimeAtTimeSync;
    pdelayReqEventEgressTimestamp += newLocalTimeAtTimeSync - oldLocalTimeAtTimeSync;

    /************** Rate ratio calculation *************************************
     * It is calculated based on interval between two successive Sync messages *
     ***************************************************************************/

    EV_INFO << "############## SYNC #####################################"<< endl;
    EV_INFO << "RECEIVED TIME AFTER SYNC   - " << newLocalTimeAtTimeSync << endl;
    EV_INFO << "RECEIVED SIM TIME          - " << now << endl;
    EV_INFO << "ORIGIN TIME SYNC           - " << peerSentTimeSync << endl;
    EV_INFO << "RESIDENCE TIME             - " << residenceTime << endl;
    EV_INFO << "CORRECTION FIELD           - " << correctionField << endl;
    EV_INFO << "PROPAGATION DELAY          - " << peerDelay << endl;
    EV_INFO << "TIME DIFFERENCE TO SIMTIME - " << CLOCKTIME_AS_SIMTIME(newLocalTimeAtTimeSync) - now << endl;
    EV_INFO << "RATE RATIO                 - " << gmRateRatio << endl;

    emit(rateRatioSignal, gmRateRatio);
    emit(correctionFieldSignal, CLOCKTIME_AS_SIMTIME(correctionField));
    emit(localTimeSignal, CLOCKTIME_AS_SIMTIME(newLocalTimeAtTimeSync));
    emit(timeDifferenceSignal, CLOCKTIME_AS_SIMTIME(newLocalTimeAtTimeSync) - now);
}

void GptpTSN::receiveSignal(cComponent *source, simsignal_t simSignal, cObject *obj, cObject *details)
{
    Enter_Method("%s", cComponent::getSignalName(simSignal));

    if (simSignal != receptionEndedSignal && simSignal != transmissionEndedSignal)
        return;

    auto ethernetSignal = check_and_cast<cPacket *>(obj);
    auto packet = check_and_cast_nullable<Packet *>(ethernetSignal->getEncapsulatedPacket());
    if (!packet)
        return;

    auto gptp = extractGptpHeader(packet);
    if (!gptp)
        return;

    if (gptp->getDomainNumber() != domainNumber)
        return;

    if (simSignal == receptionEndedSignal)
        packet->addTagIfAbsent<GptpIngressTimeInd>()->setArrivalClockTime(clock->getClockTime());
    else if (simSignal == transmissionEndedSignal)
        handleDelayOrSendFollowUp(gptp, source);
}

const GptpBase *GptpTSN::extractGptpHeader(Packet *packet)
{
    auto protocol = packet->getTag<PacketProtocolTag>()->getProtocol();
    if (*protocol != Protocol::ethernetPhy)
        return nullptr;

    const auto& ethPhyHeader = packet->peekAtFront<physicallayer::EthernetPhyHeader>();
    const auto& ethMacHeader = packet->peekDataAt<EthernetMacHeader>(ethPhyHeader->getChunkLength());
    if (ethMacHeader->getTypeOrLength() != ETHERTYPE_GPTP)
        return nullptr;

    b offset = ethPhyHeader->getChunkLength() + ethMacHeader->getChunkLength();
    return packet->peekDataAt<GptpBase>(offset).get();
}

void GptpTSN::handleDelayOrSendFollowUp(const GptpBase *gptp, cComponent *source)
{
    int portId = getContainingNicModule(check_and_cast<cModule*>(source))->getInterfaceId();

    switch (gptp->getMessageType()) {
    case GPTPTYPE_PDELAY_REQ:
        if (portId == slavePortId)
            pdelayReqEventEgressTimestamp = clock->getClockTime();
        break;
    case GPTPTYPE_PDELAY_RESP: {
        auto gptpResp = check_and_cast<const GptpPdelayResp*>(gptp);
        sendPdelayRespFollowUp(portId, gptpResp);
        break;
    }
    case GPTPTYPE_SYNC: {
        auto gptpSync = check_and_cast<const GptpSync*>(gptp);
        if(gptpNodeType == MASTER_NODE)
            peerSentTimeSync = clock->getClockTime();
        sendFollowUp(portId, gptpSync, peerSentTimeSync);
        break;
    }
    default:
        break;
    }
}
