#include <string>
#include <map>
#include <fstream>
#include <sstream>
#include <cmath>
#include <vector>
#include "ns3/log.h"
#include "ns3/simulator.h"
#include "ns3/node-list.h"
#include "ns3/node.h"
#include "ns3/ipv4.h"
#include "ns3/constant-velocity-mobility-model.h"
#include "ns3/ns2-waypoint-mobility-helper.h"

NS_LOG_COMPONENT_DEFINE("Ns2WaypointMobilityHelper");

using namespace std;

namespace ns3 {

    typedef struct parse_result {
        vector<string> tokens; // strings separated by space (this has 4 tokens)
        vector<int> ivals;
        vector<bool> has_ival;
        vector<double> dvals;
        vector<bool> has_dval;
        vector<string> svals;
    } parse_result;

    parse_result *parse(const string& str);
    string num(const string& str);
    template<class T>
    bool val(const string& str, T& ret);
    string trim(const string& s);

    Ns2WaypointMobilityHelper::Ns2WaypointMobilityHelper(bool assume2d, bool ignorePositioning, bool positioningImpliesStop)
    {
        this->assume2d = assume2d;
        this->ignorePositioning = ignorePositioning;
        this->positioningImpliesStop = positioningImpliesStop;
    }

    Ptr<ConstantVelocityMobilityModel>
    Ns2WaypointMobilityHelper::GetMobilityModel(std::string idString, const ObjectStore &store) const {
        std::istringstream iss;
        iss.str(idString);
        uint32_t id;
        iss >> id;
        Ptr<Object> object = store.Get(id);
        if (object == 0) {
            return 0;
        }
        Ptr<ConstantVelocityMobilityModel> model = object->GetObject<ConstantVelocityMobilityModel > ();
        if (model == 0) {
            model = CreateObject<ConstantVelocityMobilityModel > ();
            object->AggregateObject(model);
        }
        return model;
    }

    void
    Ns2WaypointMobilityHelper::LayoutObjectStore(const ObjectStore &store) const {
        unsigned int offset=0;
        double oldTime[2000] = {0};

        for (unsigned int i = 0; i < mobilityTraces.size(); i++) {
            unsigned int max_id=-1;
            std::ifstream file(mobilityTraces[i].c_str(), std::ios::in);
            map<int, Vector> last_pos;
            if (file.is_open()) {
                while (!file.eof()) {
                    std::string line;
                    getline(file, line);

                    //ignore comments (#)
                    size_t pos_sharp = line.find('#');
                    if (pos_sharp != string::npos) line = line.substr(0, pos_sharp);
                    line = trim(line);

                    parse_result *pr = parse(line);
                    //line like "$node_(0) set X_ 28.675920486450"
                    if (pr->tokens.size() == 4 && pr->tokens[0].find("$node_(") == 0 && pr->tokens[1] == "set") {
                        if (!pr->has_dval[3]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_dval[3]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_ival[0]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_ival[0]");
                            delete pr;
                            continue;
                        }
                        //adjust w.r.t. offset & update max-id
                        pr->ivals[0]+=offset;
                        ostringstream s;
                        s<<pr->ivals[0];
                        pr->svals[0]=s.str();
                        if(pr->ivals[0]>(int)max_id) max_id=pr->ivals[0];
                        Ptr<ConstantVelocityMobilityModel> model = GetMobilityModel(pr->svals[0], store);
                        if (0 == model) {
                            NS_LOG_DEBUG("rejected line " << line << ": no model");
                            delete pr;
                            continue;
                        }
                        char coord = pr->tokens[2][0]; //'X' or 'Y'
                        Vector position = model->GetPosition();
                        switch (coord) {
                            case 'X':
                                position.x = pr->dvals[3];
                                model->SetPosition(position);
                                last_pos[pr->ivals[0]] = position;
                                NS_LOG_DEBUG("accepted line " << line << ": setx");
                                break;
                            case'Y':
                                position.y = pr->dvals[3];
                                model->SetPosition(position);
                                last_pos[pr->ivals[0]] = position;
                                NS_LOG_DEBUG("accepted line " << line << ": sety");
                                break;
                            default:
                                NS_LOG_DEBUG("rejected line " << line << ": no x or y: " << coord);
                                break;
                        }
                    }
                    //line like "$ns_ at 4.634906291962 "$node_(0) set X_ 28.675920486450""
                    if (pr->tokens.size() == 7 && pr->tokens[0] == "$ns_" && pr->tokens[1] == "at" && pr->tokens[4] == "set") {
                        if (!pr->has_dval[6]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_dval[6]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_ival[3]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_ival[3]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_dval[2]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_ival[2]");
                            delete pr;
                            continue;
                        }
                        //adjust w.r.t. offset & update max-id
                        pr->ivals[3]+=offset;
                        ostringstream s;
                        s<<pr->ivals[3];
                        pr->svals[3]=s.str();
                        if(pr->ivals[3]>(int)max_id) max_id=pr->ivals[3];
                        if (ignorePositioning) {
                            NS_LOG_DEBUG("rejected line " << line << ": ignore positioning");
                            delete pr;
                            continue;
                        }
                        Ptr<ConstantVelocityMobilityModel> model = GetMobilityModel(pr->svals[3], store);
                        if (0 == model) {
                            NS_LOG_DEBUG("rejected line " << line << ": no model");
                            delete pr;
                            continue;
                        }
                        char coord = pr->tokens[5][0]; //'X' or 'Y'
                        Vector position = model->GetPosition();
                        switch (coord) {
                            case 'X':
                                position.x = pr->dvals[6];
                                Simulator::Schedule(Seconds(pr->dvals[2]), &Ns2WaypointMobilityHelper::setPosition, this, model, position);
                                if (positioningImpliesStop) Simulator::Schedule(Seconds(pr->dvals[2]), &Ns2WaypointMobilityHelper::setVelocity, this, model, Vector(0, 0, 0));
                                last_pos[pr->ivals[3]] = position;
                                NS_LOG_DEBUG("accepted line " << line << ": schedule setx");
                                break;
                            case'Y':
                                position.y = pr->dvals[6];
                                Simulator::Schedule(Seconds(pr->dvals[2]), &Ns2WaypointMobilityHelper::setPosition, this, model, position);
                                if (positioningImpliesStop) Simulator::Schedule(Seconds(pr->dvals[2]), &Ns2WaypointMobilityHelper::setVelocity, this, model, Vector(0, 0, 0));
                                last_pos[pr->ivals[3]] = position;
                                NS_LOG_DEBUG("accepted line " << line << ": schedule sety");
                                break;
                            default:
                                NS_LOG_DEBUG("rejected line " << line << ": no x or y");
                                break;
                        }
                    }

// added to try switching on/off the interface
                    //line like "$ns_ at 16.020000 "$node_(5) switch OFF""
                    //line like "$ns_ at 0.010000 "$node_(5) switch ON""
                    //              0  1     2         3       4     5
                    if (pr->tokens.size() == 6 && pr->tokens[0] == "$ns_" && pr->tokens[1] == "at" && pr->tokens[4] == "switch") {
                        if (!pr->has_ival[3]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_ival[3]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_dval[2]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_ival[2]");
                            delete pr;
                            continue;
                        }
                        //adjust w.r.t. offset & update max-id
                        pr->ivals[3]+=offset;
                        ostringstream s;
                        s<<pr->ivals[3];
                        pr->svals[3]=s.str();
                        if(pr->ivals[3]>(int)max_id) max_id=pr->ivals[3];
                        Ptr<ConstantVelocityMobilityModel> model = GetMobilityModel(pr->svals[3], store);
                        if (0 == model) {
                            NS_LOG_DEBUG("rejected line " << line << ": no model");
                            delete pr;
                            continue;
                        }
                        char ifStatus = pr->tokens[5][1]; //'N' or 'F'
                        switch (ifStatus) {
                            case 'N':// O'N'
                                Simulator::Schedule (Seconds(pr->dvals[2]),&Ipv4::SetUp, (NodeList::GetNode(pr->ivals[3]+m_nRSU))->GetObject<Ipv4> (), 1);
                                NS_LOG_DEBUG("accepted line " << line << ": switch ON for " << (pr->ivals[3]+m_nRSU));
                                break;
                            case'F'://O'F'F
                                Simulator::Schedule (Seconds(pr->dvals[2]),&Ipv4::SetDown, (NodeList::GetNode(pr->ivals[3]+m_nRSU))->GetObject<Ipv4> (), 1);
                                NS_LOG_DEBUG("accepted line " << line << ": switch OFF for " << (pr->ivals[3]+m_nRSU));
                                break;
                            default:
                                NS_LOG_DEBUG("rejected line " << line << ": no N or F");
                                break;
                        }
                    }
// end add on/off interface

                    //line like "$ns_ at 4.634906291962 "$node_(0) setdest 44.610576629639 52.860397338867 0.917085826397"";
                    //            0   1   2 (time)        3         4       5 (pos.x)       6 (pos.y)       7 (velocity)
                    if (pr->tokens.size() == 8 && pr->tokens[0] == "$ns_" && pr->tokens[1] == "at" && pr->tokens[4] == "setdest") {
                        if (!pr->has_dval[2]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_dval[2]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_ival[3]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_ival[3]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_dval[5]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_dval[5]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_dval[6]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_dval[6]");
                            delete pr;
                            continue;
                        }
                        if (!pr->has_dval[7]) {
                            NS_LOG_DEBUG("rejected line " << line << ": !pr->has_dval[7]");
                            delete pr;
                            continue;
                        }
                        //adjust w.r.t. offset & update max-id
                        pr->ivals[3]+=offset;
                        ostringstream s;
                        s<<pr->ivals[3];
                        pr->svals[3]=s.str();
                        if(pr->ivals[3]>(int)max_id) max_id=pr->ivals[3];
                        Vector vel, posDest; //added posDest
                        double time;
                        int nodeId = pr->ivals[3];
                        Ptr<ConstantVelocityMobilityModel> model = GetMobilityModel(pr->svals[3], store);
                        if (0 == model) {
                            NS_LOG_DEBUG("rejected line " << line << ": no model");
                            delete pr;
                            continue;
                        }
                        if (assume2d) {
                            vel.z = 0;
                            posDest.x = pr->dvals[5];
                            posDest.y = pr->dvals[6];
                            posDest.z = 0;
                            double dx = pr->dvals[5] - last_pos[pr->ivals[3]].x; //dv5: dest x
                            double dy = pr->dvals[6] - last_pos[pr->ivals[3]].y; //dv6: dest y
                            time = sqrt(dx * dx + dy * dy) / pr->dvals[7]; //dv7: speed module
                            vel.x = dx / time; // v=d/t
                            vel.y = dy / time; // v=d/t
                            last_pos[pr->ivals[3]] = Vector(pr->dvals[5], pr->dvals[6], 0);
                        }else {
                            vel.x = pr->dvals[5];
                            vel.y = pr->dvals[6];
                            vel.z = pr->dvals[7];
                            time = -1;
                        }
                        NS_LOG_DEBUG("current " << pr->dvals[2] << " old " << oldTime[nodeId]);
                        if ((oldTime[nodeId] != 0) && (oldTime[nodeId] >= pr->dvals[2]))
                        {
                          pr->dvals[2] = oldTime[nodeId] + 0.0001;
                          NS_LOG_DEBUG("$node_(" << nodeId << ")adjust time to prevent gaps -- new time scheduled is " << pr->dvals[2]); 
                        }
                        Simulator::Schedule(Seconds(pr->dvals[2]), &Ns2WaypointMobilityHelper::setVelocity, this, model, vel);
                        if (time >= 0) Simulator::Schedule(Seconds(pr->dvals[2] + time), &Ns2WaypointMobilityHelper::setVelocity, this, model, Vector(0, 0, 0));
                        if (time >= 0) Simulator::Schedule(Seconds(pr->dvals[2] + time), &Ns2WaypointMobilityHelper::setPosition, this, model, posDest);
                       NS_LOG_DEBUG("accepted line " << line << ": sched move, end " << pr->dvals[2] + time);
                       NS_LOG_DEBUG("$node_(" << nodeId << ") set movement to stop at " << posDest << " at time " << pr->dvals[2] + time << " oldTime= " << oldTime[nodeId]);
                       oldTime[nodeId] = pr->dvals[2] + time;
                    }
                    delete pr;
                }
                file.close();
                offset=max_id+1;
            }
        }
    }

    void
    Ns2WaypointMobilityHelper::Install(void) const {
        Install(NodeList::Begin(), NodeList::End());
    }
    
    void Ns2WaypointMobilityHelper::Install (NodeContainer container){
        m_nRSU = 0;
        Install(container.Begin(),container.End());
    }

    void Ns2WaypointMobilityHelper::Install (NodeContainer container, uint32_t nRSU){
        m_nRSU = nRSU;
        Install(container.Begin(),container.End());
    }

    bool& Ns2WaypointMobilityHelper::Assume2d(void) {
        return assume2d;
    }

    bool& Ns2WaypointMobilityHelper::IngorePositioning(void) {
        return ignorePositioning;
    }

    bool& Ns2WaypointMobilityHelper::PositioningImpliesStop(void) {
        return positioningImpliesStop;
    }

    void Ns2WaypointMobilityHelper::setPosition(Ptr<ConstantVelocityMobilityModel> model, const Vector& pos) const {
        model->SetPosition(pos);
        NS_LOG_DEBUG("set position: " << pos);
    }

    void Ns2WaypointMobilityHelper::setVelocity(Ptr<ConstantVelocityMobilityModel> model, const Vector& vel) const {
        model->SetVelocity(vel);
        NS_LOG_DEBUG("set velocity: " << vel << " (position is " << model->GetPosition() << ")");
    }

    parse_result *parse(const string& str) {
        parse_result *ret = new parse_result();
        istringstream s;
        s.str(str);
        while (!s.eof()) {
            string x;
            s >> x;
            ret->tokens.push_back(x);
            int ii;
            double d;
            ret->has_ival.push_back(val<int>(x, ii));
            ret->ivals.push_back(ii);
            ret->has_dval.push_back(val<double>(x, d));
            ret->dvals.push_back(d);
            ret->svals.push_back(num(x));
        }
        return ret;
    }

    string num(const string& str) {
        string ret;
        for (unsigned int i = 0; i < str.size(); i++) {
            if (isdigit(str[i]) || str[i] == '.') ret += str[i];
        }
        return ret;
    }

    template<class T>
    bool val(const string& str, T& ret) {
        string s2 = num(str);
        if (s2.size() == 0) return false;
        istringstream s(s2);
        s >> ret;
        return true;
    }

    string trim(const string& s) {
        string ret=s;
        while(ret.size()>0 && isblank(ret[0])) ret.erase(0,1);
        while(ret.size()>0 && isblank(ret[ret.size()-1])) ret.erase(ret.size()-1,1);
        return ret;
    }
    
    vector<string>& Ns2WaypointMobilityHelper::MobilityTraces(void){
        return mobilityTraces;
    }


} // namespace ns3