/* 
 * File:   ziehl_tr1200ip_simulator.cpp
 * Author: Jan Hudec
 * 
 * Created on 25. dubna 2019, 18:00
 */

#include "ziehl_tr1200ip_simulator.h"

// include necessary libraries
#include <fstream>
#include <iostream>
#include <sstream>
#include <time.h>
#include <math.h>

ziehl_tr1200ip_simulator::ziehl_tr1200ip_simulator()
{
    // seed random function
    srand(time(NULL));

    // initialize class variables
    this->_temperatures = { };
    this->_static_temp_values = {
        std::pair<int, float>(0, 20.1f),
        std::pair<int, float>(1, 19.4f),
        std::pair<int, float>(2, 18.7f),
        std::pair<int, float>(3, 18.5f),
        std::pair<int, float>(4, 18.3f),
        std::pair<int, float>(5, 18.1f),
        std::pair<int, float>(6, 17.8f),
        std::pair<int, float>(7, 18.6f),
        std::pair<int, float>(8, 20.6f),
        std::pair<int, float>(9, 22.8f),
        std::pair<int, float>(10, 23.9f),
        std::pair<int, float>(11, 25.8f),
        std::pair<int, float>(12, 24.9f),
        std::pair<int, float>(13, 25.8f),
        std::pair<int, float>(14, 26.9f),
        std::pair<int, float>(15, 27.5f),
        std::pair<int, float>(16, 27.1f),
        std::pair<int, float>(17, 26.3f),
        std::pair<int, float>(18, 26.2f),
        std::pair<int, float>(19, 24.5f),
        std::pair<int, float>(20, 24.0f),
        std::pair<int, float>(21, 23.5f),
        std::pair<int, float>(22, 22.3f),
        std::pair<int, float>(23, 21.1f),
    };
    this->_interface = "ens33";
    this->_vlan_priority = 4;
    this->_vlan_id = 0;
    this->_sv_app_id = 0x4000;
    this->_sv_mac[0] = 0x00;
    this->_sv_mac[1] = 0x00;
    this->_sv_ratio = 100;
    this->_goose_app_id = 0x1000;
    this->_goose_mac[0] = 0x00;
    this->_goose_mac[1] = 0x00;
    this->_goose_time_min = 5;
    this->_goose_time_max = 100;
    this->_deadband = 2.0f;
    this->_sensors = 1;
    this->_temp_min = 15;
    this->_temp_max = 30;
    this->_time_factor = 1;
    this->_t = time(NULL);
    this->_ti = *localtime(&this->_t);
    this->_monitoring_time = 1;
}

ziehl_tr1200ip_simulator::~ziehl_tr1200ip_simulator()
{
    SVPublisher_destroy(this->_sv_pub);
    GoosePublisher_destroy(this->_gp_pub);
    LinkedList_destroyDeep(this->_dataset, (LinkedListValueDeleteFunction) MmsValue_delete);
    LinkedList_destroyDeep(this->_dataset_sp, (LinkedListValueDeleteFunction) MmsValue_delete);
}

void ziehl_tr1200ip_simulator::parse_config(std::string filename)
{
    // load config file
    std::ifstream file(filename);
    std::string str;
    std::vector<std::string> parse;

    // read content line by line
    while (std::getline(file, str))
    {
        // skip commented lines
        if (str[0] != '#')
        {
            // split to key and value by '=' char
            int pos = str.find('=');
            if (pos != -1)
            {
                parse.clear();
                parse.push_back(str.substr(0, pos));
                parse.push_back(str.substr(pos + 1, str.length()));

                // assign values to class variables
                if      (parse[0] == "interface")       this->_interface = parse[1];
                else if (parse[0] == "vlan_priority")   this->_vlan_priority = stoi(parse[1]);
                else if (parse[0] == "vlan_id")         this->_vlan_id = stoi(parse[1]);
                else if (parse[0] == "sv_app_id")       this->_sv_app_id = strtol(parse[1].c_str(), NULL, 16);
                else if (parse[0] == "sv_mac_5")        this->_sv_mac[0] = strtol(parse[1].c_str(), NULL, 16);
                else if (parse[0] == "sv_mac_6")        this->_sv_mac[1] = strtol(parse[1].c_str(), NULL, 16);
                else if (parse[0] == "sv_ratio")        this->_sv_ratio = stoi(parse[1]);
                else if (parse[0] == "goose_app_id")    this->_goose_app_id = strtol(parse[1].c_str(), NULL, 16);
                else if (parse[0] == "goose_mac_5")     this->_goose_mac[0] = strtol(parse[1].c_str(), NULL, 16);
                else if (parse[0] == "goose_mac_6")     this->_goose_mac[1] = strtol(parse[1].c_str(), NULL, 16);
                else if (parse[0] == "goose_time_min")  this->_goose_time_min = stoi(parse[1]);
                else if (parse[0] == "goose_time_max")  this->_goose_time_max = stoi(parse[1]);
                else if (parse[0] == "deadband")        this->_deadband = stof(parse[1]);
                else if (parse[0] == "monitoring_time") this->_monitoring_time = stoi(parse[1]);
                else if (parse[0] == "time_factor")     this->_time_factor = stoi(parse[1]);
                else if (parse[0] == "sensors")         this->_sensors = stoi(parse[1]);				
                else if (parse[0] == "temp_min")        this->_temp_min = stoi(parse[1]);				
                else if (parse[0] == "temp_max")        this->_temp_max = stoi(parse[1]);
                else if (parse[0] == "year")            this->_ti.tm_year = stoi(parse[1]) - 1900;
                else if (parse[0] == "month")           this->_ti.tm_mon = stoi(parse[1]) - 1;
                else if (parse[0] == "day")             this->_ti.tm_mday = stoi(parse[1]);
                else if (parse[0] == "hours")           this->_ti.tm_hour = stoi(parse[1]);
                else if (parse[0] == "minutes")         this->_ti.tm_min = stoi(parse[1]);
                else if (parse[0] == "seconds")         this->_ti.tm_sec = stoi(parse[1]);
            }
        }
    }

    // check monitoring_time value
    if(this->_monitoring_time < 1)
        throw std::runtime_error("Monitoring time has to be at least 1 second!");

    // check time_factor value
    if (this->_time_factor < 1)
        throw std::runtime_error("Time factor value has to be at least 1!");
    
    // check goose_time_min value
    if (this->_goose_time_min < 1)
        throw std::runtime_error("Minimum time for spontaneous GOOSE transmit has to be at least 1 millisecond!");
    
    // check goose_time_max value
    if (this->_goose_time_max < 5)
        throw std::runtime_error("Time for regular GOOSE transmit has to be at least 5 milliseconds!");
    
    if (this->_goose_time_max < this->_goose_time_min)
        throw std::runtime_error("Time for regular GOOSE transmit has to be bigger than minimum time!");

    // create storage vectors or throw exception when more than 12 sensors
    if (this->_sensors > 12)
        throw std::runtime_error("Too many sensors!");

    // check time values
    if (70 <= this->_ti.tm_year && this->_ti.tm_year <= 138 &&
        0 <= this->_ti.tm_mon  && this->_ti.tm_mon  <= 11  &&
        1 <= this->_ti.tm_mday && this->_ti.tm_mday <= 31  &&
        0 <= this->_ti.tm_hour && this->_ti.tm_hour <= 23  &&
        0 <= this->_ti.tm_min  && this->_ti.tm_min  <= 59  &&
        0 <= this->_ti.tm_sec  && this->_ti.tm_sec  <= 60)
    {
        // update timestamp variable
        this->_t = mktime(&this->_ti);

        for (int i = 0; i < this->_sensors; i++)
        {
            if (7 >= this->_ti.tm_hour && this->_ti.tm_hour <= 15)
                this->_temperatures.push_back({ std::pair<time_t, float>(0, 1) });
            else
                this->_temperatures.push_back({ std::pair<time_t, float>(0, 0) });
        }
    }
    else
    {
        throw std::runtime_error("Wrong time values!");
    }
    
    init_iec();
    
    // create asdu for each sensor
    std::string name;
    for(int i = 0; i < this->_sensors; i++)
    {
        name = "TR1200IP_" + std::to_string(i + 1);
        
        // create asdu
        SVPublisher_ASDU asdu = SVPublisher_addASDU(this->_sv_pub, name.c_str(), NULL, 1);
        this->_asdu_units.insert(std::pair<std::string, SVPublisher_ASDU>(name, asdu));
        this->_asdu_units_offsets[name] = {};
           
        // reserve memory for int
        this->_asdu_units_offsets[name].push_back(
            SVPublisher_ASDU_addINT8(this->_asdu_units[name])
        );
        
        // reserve memory for float
        this->_asdu_units_offsets[name].push_back(
            SVPublisher_ASDU_addFLOAT(this->_asdu_units[name])
        );
        
        // reserve memory for quality
        this->_asdu_units_offsets[name].push_back(
            SVPublisher_ASDU_addQuality(this->_asdu_units[name])
        );
        
        // reserve memory for timestamp
        this->_asdu_units_offsets[name].push_back(
            SVPublisher_ASDU_addTimestamp(this->_asdu_units[name])
        );
    }

    SVPublisher_setupComplete(this->_sv_pub);
}

void ziehl_tr1200ip_simulator::init_iec()
{
    char* interface = strdup(this->_interface.data());
    interface[strlen(interface) - 1] = '\0';
    
    uint8_t sv_mac_4 = 0x04;
    uint8_t goose_mac_4 = 0x01;
    
    CommParameters cp;
    cp.vlanPriority = this->_vlan_priority;
    cp.vlanId = this->_vlan_id;
    cp.appId = this->_sv_app_id;
    cp.dstAddress[0] = 0x01;
    cp.dstAddress[1] = 0x0c;
    cp.dstAddress[2] = 0xcd;
    cp.dstAddress[3] = sv_mac_4;
    cp.dstAddress[4] = this->_sv_mac[0];
    cp.dstAddress[5] = this->_sv_mac[1];
    
    this->_sv_pub = SVPublisher_create(&cp, interface);
    
    cp.appId = this->_goose_app_id;
    cp.dstAddress[3] = goose_mac_4;
    cp.dstAddress[4] = this->_goose_mac[0];
    cp.dstAddress[5] = this->_goose_mac[1];
    
    this->_gp_pub = GoosePublisher_create(&cp, interface);
    
    GoosePublisher_setGoID(this->_gp_pub, "ZIEHL_TR1200IP");
    GoosePublisher_setGoCbRef(this->_gp_pub, "TR1200IPMEAS/LLN0$GO$TempAndRelay");
    GoosePublisher_setDataSetRef(this->_gp_pub, "TR1200IPMEAS/LLN0$TempAndRelay");
    GoosePublisher_setTimeAllowedToLive(this->_gp_pub, (1.5 * this->_goose_time_max));
    GoosePublisher_setConfRev(this->_gp_pub, 1);
    GoosePublisher_setSimulation(this->_gp_pub, 0);
    GoosePublisher_setNeedsCommission(this->_gp_pub, 0);

    this->_goose_counter = 0;
    this->time_treshold = this->_goose_time_max;
    this->goose_time_treshold = -1;
    
    this->_dataset = LinkedList_create();
    this->_dataset_sp = LinkedList_create();
}

void ziehl_tr1200ip_simulator::save_temperatures(std::string filename)
{
    // create stringstream and write all values accordingly
    std::stringstream ss;
    ss.fixed;
    ss.precision(4);

    std::map<time_t, float>::iterator it;

    for (int i = 0; i < this->_sensors; i++)
    {
        ss << i + 1 << ":";

        for (it = ++this->_temperatures[i].begin(); it != this->_temperatures[i].end(); it++)
        {
            ss << it->first << "=" << it->second << ",";
        }

        // move write head one char back so last comma will be overwritten
        ss.seekp(-1, ss.cur);
        ss << std::endl;
    }
    std::string s = ss.str();
    //std::cout << s; // debug output of values

    // save output to file
    std::ofstream outfile(filename);
    outfile << ss.rdbuf();
}

void ziehl_tr1200ip_simulator::generate_temperatures()
{
    float temp_last,
        temp_growth,
        temp_new,
        rand_low,
        rand_high;

    // assign low and high values for random function based on statistical data
    rand_low = 0.000055f * this->_monitoring_time * this->_time_factor;
    rand_high = 0.00061f * this->_monitoring_time * this->_time_factor;

    std::map<time_t, float>::iterator it;
    
    // remove old entries from GOOSE dataset
    LinkedList_destroy(this->_dataset);
    this->_dataset = LinkedList_create();
    
    for (int i = 0; i < this->_sensors; i++)
    {
        // if temperatures are empty assign first value based on hour of day
        // else use last generated value
        if (this->_temperatures[i].size() == 1)
        {
            temp_last = this->_static_temp_values[this->_ti.tm_hour];
        }
        else
        {
            it = --this->_temperatures[i].end();
            temp_last = it->second;
        }

        // positive temperature growth during day, negative during night
        if (6 <= this->_ti.tm_hour && this->_ti.tm_hour <= 17)
            this->_temperatures[i][0] = 1;
        else
            this->_temperatures[i][0] = 0;

        // if temperature changes out of bounds, switch growth to keep it normal
        if (temp_last < this->_temp_min)
            this->_temperatures[i][0] = 1;
        else if (temp_last > this->_temp_max)
            this->_temperatures[i][0] = 0;

        // generate temperature growth and assign correct positive/negative sign accordingly
        temp_growth = rand_low + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / (rand_high - rand_low)));
        temp_growth = (this->_temperatures[i][0] == 1) ? temp_growth : -temp_growth;

        // insert new value to map
        temp_new = temp_last + temp_growth;
        this->_temperatures[i].insert(std::pair<time_t, float>(this->_t, temp_new));
        
        // prepare sample values
        std::string name = "TR1200IP_" + std::to_string(i + 1);
        
        SVPublisher_ASDU_setINT8(
            this->_asdu_units[name],
            this->_asdu_units_offsets[name][0],
            i + 1
        );
        
        SVPublisher_ASDU_setFLOAT(
            this->_asdu_units[name],
            this->_asdu_units_offsets[name][1],
            temp_new
        );

        SVPublisher_ASDU_setQuality(
            this->_asdu_units[name],
            this->_asdu_units_offsets[name][2],
            QUALITY_VALIDITY_GOOD
        );

        Timestamp ts;
        Timestamp_setTimeInSeconds(&ts, this->_t);
        SVPublisher_ASDU_setTimestamp(
            this->_asdu_units[name],
            this->_asdu_units_offsets[name][3],
            ts
        );
        
        // add values to GOOSE dataset
        LinkedList_add(this->_dataset, MmsValue_newIntegerFromInt8(i + 1));
        LinkedList_add(this->_dataset, MmsValue_newFloat(temp_new));
        LinkedList_add(this->_dataset, MmsValue_newUtcTime(this->_t));
                
        // check for delta temperature bigger than deadband and send unsolicited GOOSE message
        if(std::abs(temp_growth) >= this->_deadband)
        {
            printf("\nAlert - temperature growth higher than deadband!\n"
                   "  Growth: %f\tDeadband: %f\tID: %i\tTimestamp: %ld \n", temp_growth, this->_deadband, i + 1, static_cast<long>(this->_t));
            
            // increase stNum and reset sqNum
            GoosePublisher_increaseStNum(this->_gp_pub);
            
            // initialize retransmiting of GOOSE message
            retransmit_goose(i, temp_new, this->_t);
        }
    }

    // update timestamp and save it into tm struct
    this->_t += this->_monitoring_time * this->_time_factor;
    this->_ti = *localtime(&this->_t);
    
    // publish sv every x GOOSE messages
    if(this->_goose_counter % this->_sv_ratio == 0)
    {
        SVPublisher_publish(this->_sv_pub);
        
        // increment SmpCnt counter
        std::map<std::string, SVPublisher_ASDU>::iterator it2;
        for(it2 = this->_asdu_units.begin(); it2 != this->_asdu_units.end(); it2++)
        {
            SVPublisher_ASDU_increaseSmpCnt(it2->second);
        }
    }
    
    // publish GOOSE frame
    GoosePublisher_publish(this->_gp_pub, this->_dataset);
    
    // increment goose_counter
    this->_goose_counter += 1;
}

void ziehl_tr1200ip_simulator::retransmit_goose(int i, float temp, time_t t)
{
    static int iterations;
    
    if(iterations == 0)
    {
        while(this->_goose_time_min * std::pow(2, iterations) < this->_goose_time_max)
        {
            iterations += 1;   
        }
    }
    
    for(int j = 0; j < iterations + 1; j++)
    {
        // remove old entries from GOOSE dataset_sp
        LinkedList_destroy(this->_dataset_sp);
        this->_dataset_sp = LinkedList_create();

        // prepare new GOOSE messages
        LinkedList_add(this->_dataset_sp, MmsValue_newIntegerFromInt8(i + 1));
        LinkedList_add(this->_dataset_sp, MmsValue_newFloat(temp));
        LinkedList_add(this->_dataset_sp, MmsValue_newUtcTime(t));

        // publish immediately
        GoosePublisher_publish(this->_gp_pub, this->_dataset_sp);
        
        // wait for double time each iteration
        if(j < iterations)
        {
            uint64_t time_old = Hal_getTimeInMs();
            while(Hal_getTimeInMs() - time_old < this->_goose_time_min * std::pow(2, j));
        }
    }
}

// async spontaneous GOOSE transmit - causes spontaneous and regular GOOSE frames mish-mash
/*void ziehl_tr1200ip_simulator::retransmit_goose(int i, float temp, time_t t)
{
    static int iteration;
    static int id;
    static float temperature;
    static time_t time;
    
    // if all values are zero, function is in the middle of retransmitting GOOSE frames
    if(i == 0 && temp == 0 && t == 0)
    {
        // or has not started yet
        if(iteration != -1)
        {
            // if this iteration is still inside of time window for retransmit
            if (this->_goose_time_min * std::pow(2, iteration) < this->_goose_time_max * 2)
            {
                // remove old entries from GOOSE dataset
                LinkedList_destroy(this->_dataset_sp);
                this->_dataset_sp = LinkedList_create();

                // prepare new GOOSE messages
                LinkedList_add(this->_dataset_sp, MmsValue_newIntegerFromInt8(id + 1));
                LinkedList_add(this->_dataset_sp, MmsValue_newFloat(temperature));
                LinkedList_add(this->_dataset_sp, MmsValue_newUtcTime(time));

                // publish immediately
                GoosePublisher_publish(this->_gp_pub, this->_dataset_sp);

                // set time treshold for loop in main.cpp
                this->goose_time_treshold = this->_goose_time_min * std::pow(2, iteration + 1);
                
                // increment iteration counter
                iteration += 1;
            }
            else
            {
                // reset iteration counter
                iteration = -1;
                
                // reset time treshold for loop in main.cpp
                this->goose_time_treshold = -1;
            }
        }
    }
    // new values to retransmit
    else
    {
        iteration = 0;
        id = i;
        temperature = temp;
        time = t;
        this->goose_time_treshold = this->_goose_time_min;
    }
}*/