/*
Copyright (C) 2012  Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU 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 General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>       /* for time */
#include <errno.h>
#include <assert.h>
#include <fcntl.h>
#include <libconfig.h>  /* read config files */
#include <unistd.h>     /* for getopt */
#include <termios.h>    /* used in kbhit() */
#include <getopt.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <pthread.h>
#include <termios.h>
#include "msgqueue.h"
#include "portthread.h"
#include "network_layer.h"
#include "ipmodule.h"
#include "mstpmodule.h"
#include "bip.h"
#include "dlmstp.h"

#define KEY_ESC 27

ROUTER_PORT *head = NULL;       /* pointer to list of router ports */

int port_count;

void print_help(
    );

bool read_config(
    char *filepath);

bool parse_cmd(
    int argc,
    char *argv[]);

void init_port_threads(
    ROUTER_PORT * port_list);

bool init_router(
    );

void cleanup(
    );

void print_msg(
    BACMSG * msg);

uint16_t process_msg(
    BACMSG * msg,
    MSG_DATA * data,
    uint8_t ** buff);

uint16_t get_next_free_dnet(
    );

int kbhit(
    );

inline bool is_network_msg(
    BACMSG * msg);

int main(
    int argc,
    char *argv[])
{
    printf("I am router\n");

    ROUTER_PORT *port;
    BACMSG msg_storage, *bacmsg = NULL;
    MSG_DATA *msg_data = NULL;
    uint8_t *buff = NULL;
    int16_t buff_len = 0;

    atexit(cleanup);

    if (!parse_cmd(argc, argv)) {
        printf("parse cmd failed\r\n");
        return -1;
    }

    if (!init_router()) {
        printf("init_router failed\r\n");
        return -1;
    }


    send_network_message(NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK, msg_data,
        &buff, NULL);

    while (true) {
        if (kbhit()) {
            char ch = getchar();
            if (ch == KEY_ESC) {
                PRINT(INFO, "Received shutdown. Exiting...\n");
                break;
            }
        }

        bacmsg = recv_from_msgbox(head->main_id, &msg_storage);
        if (bacmsg) {
            switch (bacmsg->type) {
                case DATA:
                    {
                        MSGBOX_ID msg_src = bacmsg->origin;

                        /* allocate message structure */
                        msg_data = malloc(sizeof(MSG_DATA));
                        if (!msg_data) {
                            PRINT(ERROR, "Error: Could not allocate memory\n");
                            break;
                        }

                        print_msg(bacmsg);

                        if (is_network_msg(bacmsg)) {
                            buff_len =
                                process_network_message(bacmsg, msg_data,
                                &buff);
                            if (buff_len == 0) {
                                free_data(bacmsg->data);
                                break;
                            }
                        } else {
                            buff_len = process_msg(bacmsg, msg_data, &buff);
                        }

                        /* if buff_len */
                        /* >0 - form new message and send */
                        /* =-1 - try to find next router */
                        /* other value - discard message */

                        if (buff_len > 0) {
                            /* form new message */
                            msg_data->pdu = buff;
                            msg_data->pdu_len = buff_len;
                            msg_storage.origin = head->main_id;
                            msg_storage.type = DATA;
                            msg_storage.data = msg_data;

                            print_msg(bacmsg);

                            if (is_network_msg(bacmsg)) {
                                msg_data->ref_count = 1;
                                send_to_msgbox(msg_src, &msg_storage);
                            } else if (msg_data->dest.net !=
                                BACNET_BROADCAST_NETWORK) {
                                msg_data->ref_count = 1;
                                port =
                                    find_dnet(msg_data->dest.net,
                                    &msg_data->dest);
                                send_to_msgbox(port->port_id, &msg_storage);
                            } else {
                                port = head;
                                msg_data->ref_count = port_count - 1;
                                while (port != NULL) {
                                    if (port->port_id == msg_src ||
                                        port->state == FINISHED) {
                                        port = port->next;
                                        continue;
                                    }
                                    send_to_msgbox(port->port_id,
                                        &msg_storage);
                                    port = port->next;
                                }
                            }
                        } else if (buff_len == -1) {
                            uint16_t net = msg_data->dest.net;  /* NET to find */
                            PRINT(INFO, "Searching NET...\n");
                            send_network_message
                                (NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK,
                                msg_data, &buff, &net);
                        } else {
                            /* if invalid message send Reject-Message-To-Network */
                            PRINT(ERROR, "Error: Invalid message\n");
                            free_data(msg_data);
                        }
                    }
                    break;
                case SERVICE:
                default:
                    break;
            }
        }
    }

    return 0;

}

void print_help(
    )
{
    printf("Usage: router <init_method> [init_parameters]\n" "\ninit_method:\n"
        "-c, --config <filepath>\n\tinitialize router with a configuration file (.cfg) located at <filepath>\n"
        "-D, --device <dev_type> <iface> [params]\n\tinitialize a <dev_type> device using an <iface> interface specified with\n\t[params]\n"
        "\ninit_parameters:\n"
        "-n, --network <net>\n\tspecify device network number\n"
        "-P, --port <port>\n\tspecify udp port for BIP device\n"
        "-m, --mac <mac_address> [max_master] [max_frames]\n\tspecify MSTP port parameters\n"
        "-b, --baud <baud>\n\tspecify MSTP port baud rate\n"
        "-p, --parity <None|Even|Odd>\n\tspecify MSTP port parity\n"
        "-d, --databits <5|6|7|8>\n\tspecify MSTP port databits\n"
        "-s, --stopbits <1|2>\n\tspecify MSTP port stopbits\n");
}

bool read_config(
    char *filepath)
{
    config_t cfg;
    config_setting_t *setting;
    ROUTER_PORT *current = head;
    int result, fd;

    config_init(&cfg);

    /* open configuration file */
    if (!config_read_file(&cfg, filepath)) {
        PRINT(ERROR, "Config file error: %d - %s\n", config_error_line(&cfg),
            config_error_text(&cfg));
        config_destroy(&cfg);
        return false;
    }

    /* get router "port" count */
    setting = config_lookup(&cfg, "ports");
    if (setting != NULL) {
        int count = config_setting_length(setting);
        int i;

        /* lookup and initialize router "port" parameters */
        for (i = 0; i < count; i++) {
            const char *dev_type;
            const char *iface;
            long int param;
            const char *str_param;
            config_setting_t *port = config_setting_get_elem(setting, i);

            /* create new list node to store port information */
            if (head == NULL) {
                head = (ROUTER_PORT *) malloc(sizeof(ROUTER_PORT));
                head->next = NULL;
                current = head;
            } else {
                ROUTER_PORT *tmp = current;
                current = current->next;
                current = (ROUTER_PORT *) malloc(sizeof(ROUTER_PORT));
                current->next = NULL;
                tmp->next = current;
            }

            port_count++;
            config_setting_lookup_string(port, "device_type", &dev_type);
            printf("dev_type = %s\r\n", dev_type);
            if (strcmp(dev_type, "bip") == 0) {
                current->type = BIP;

                result = config_setting_lookup_string(port, "device", &iface);
                if (result) {
                    current->iface =
                        (char *) malloc((strlen(iface) + 1) * sizeof(char));
                    strcpy(current->iface, iface);

                    /* check if interface is valid */
                    fd = socket(AF_INET, SOCK_DGRAM, 0);
                    if (fd) {
                        struct ifreq ifr;
                        strncpy(ifr.ifr_name, current->iface,
                            sizeof(ifr.ifr_name) - 1);
                        result = ioctl(fd, SIOCGIFADDR, &ifr);
                        if (result != -1) {
                            close(fd);
                        } else {
                            PRINT(ERROR,
                                "Error: Invalid interface for BIP device\n");
                            return false;
                        }
                    }
                } else {
                    current->iface = "eth0";
                }

                result =
                    config_setting_lookup_int(port, "port", (int *) &param);
                if (result) {
                    current->params.bip_params.port = param;
                } else {
                    current->params.bip_params.port = 0xBAC0;
                }
                result =
                    config_setting_lookup_int(port, "network", (int *) &param);
                if (result) {
                    current->route_info.net = param;
                } else {
                    current->route_info.net = get_next_free_dnet();
                }

            } else if (strcmp(dev_type, "mstp") == 0) {
                current->type = MSTP;

                result = config_setting_lookup_string(port, "device", &iface);
                if (result) {
                    current->iface =
                        (char *) malloc((strlen(iface) + 1) * sizeof(char));
                    strcpy(current->iface, iface);

                    /* check if interface is valid */
                    fd = open(current->iface, O_NOCTTY | O_NONBLOCK);
                    if (fd != -1) {
                        close(fd);
                    } else {
                        PRINT(ERROR,
                            "Error: Invalid interface for MSTP device\n");
                        return false;
                    }
                } else {
                    current->iface = "/dev/ttyS0";
                }
                result =
                    config_setting_lookup_int(port, "mac", (int *) &param);
                if (result) {
                    current->route_info.mac[0] = param;
                    current->route_info.mac_len = 1;
                } else {
                    current->route_info.mac[0] = 127;
                    current->route_info.mac_len = 1;
                }
                result =
                    config_setting_lookup_int(port, "max_master",
                    (int *) &param);
                if (result) {
                    current->params.mstp_params.max_master = param;
                } else {
                    current->params.mstp_params.max_master = 127;
                }
                result =
                    config_setting_lookup_int(port, "max_frames",
                    (int *) &param);
                if (result) {
                    current->params.mstp_params.max_frames = param;
                } else {
                    current->params.mstp_params.max_frames = 1;
                }
                result =
                    config_setting_lookup_int(port, "baud", (int *) &param);
                if (result) {
                    current->params.mstp_params.baudrate = param;
                } else {
                    current->params.mstp_params.baudrate = 9600;
                }
                result =
                    config_setting_lookup_string(port, "parity", &str_param);
                if (result) {
                    switch (str_param[0]) {
                        case 'E':
                            current->params.mstp_params.parity = PARITY_EVEN;
                            break;
                        case 'O':
                            current->params.mstp_params.parity = PARITY_ODD;
                            break;
                        default:
                            current->params.mstp_params.parity = PARITY_NONE;
                            break;
                    }
                } else {
                    current->params.mstp_params.parity = PARITY_NONE;
                }
                result =
                    config_setting_lookup_int(port, "databits",
                    (int *) &param);
                if (result && param >= 5 && param <= 8) {
                    current->params.mstp_params.databits = param;
                } else {
                    current->params.mstp_params.databits = 8;
                }
                result =
                    config_setting_lookup_int(port, "stopbits",
                    (int *) &param);
                if (result && param >= 1 && param <= 2) {
                    current->params.mstp_params.stopbits = param;
                } else {
                    current->params.mstp_params.stopbits = 1;
                }
                result =
                    config_setting_lookup_int(port, "network", (int *) &param);
                if (result) {
                    current->route_info.net = param;
                } else {
                    current->route_info.net = get_next_free_dnet();
                }

            } else {
                PRINT(ERROR, "Error: %s unsuported\n", dev_type);
                return false;
            }
        }
    } else {
        config_destroy(&cfg);
        return false;
    }

    config_destroy(&cfg);
    printf("cmd file parse success\r\n");
    return true;
}

bool parse_cmd(
    int argc,
    char *argv[])
{
    const char *optString = "hc:D:";
    const char *bipString = "p:n:D:";
    const char *mstpString = "m:b:p:d:s:n:D:";
    const struct option Options[] = {
        {"config", required_argument, NULL, 'c'},
        {"device", required_argument, NULL, 'D'},
        {"network", required_argument, NULL, 'n'},
        {"port", required_argument, NULL, 'P'},
        {"mac", required_argument, NULL, 'm'},
        {"baud", required_argument, NULL, 'b'},
        {"parity", required_argument, NULL, 'p'},
        {"databits", required_argument, NULL, 'd'},
        {"stopbits", required_argument, NULL, 's'},
        {"help", no_argument, NULL, 'h'},
        {NULL, no_argument, NULL, 0},
    };

    int opt, dev_opt, index, result, fd;
    ROUTER_PORT *current = head;

    if (argc < 2)
        print_help();

    /* begin checking cmd parameters */
    opt = getopt_long(argc, argv, optString, Options, &index);
    printf("opt = %c\r\n", opt);
    while (opt != -1) {
        switch (opt) {
            case 'h':
                print_help();
                return false;
                break;
            case 'c':
                return read_config(optarg);
                break;
            case 'D':

                /* create new list node to store port information */
                if (head == NULL) {
                    head = (ROUTER_PORT *) malloc(sizeof(ROUTER_PORT));
                    head->next = NULL;
                    current = head;
                } else {
                    ROUTER_PORT *tmp = current;
                    current = current->next;
                    current = (ROUTER_PORT *) malloc(sizeof(ROUTER_PORT));
                    current->next = NULL;
                    tmp->next = current;
                }

                port_count++;
                if (strcmp(optarg, "bip") == 0) {
                    current->type = BIP;

                    if (optind < argc && argv[optind][0] != '-') {
                        current->iface = argv[optind];
                    } else {
                        current->iface = "eth0";
                    }

                    /* setup default parameters */
                    current->params.bip_params.port = 0xBAC0;   /* 47808 */
                    current->route_info.net = get_next_free_dnet();

                    /* check if interface is valid */
                    fd = socket(AF_INET, SOCK_DGRAM, 0);
                    if (fd) {
                        struct ifreq ifr;
                        strncpy(ifr.ifr_name, current->iface,
                            sizeof(ifr.ifr_name) - 1);
                        result = ioctl(fd, SIOCGIFADDR, &ifr);
                        if (result != -1) {
                            close(fd);
                        } else {
                            PRINT(ERROR,
                                "Error: Invalid interface for BIP device \n");
                            return false;
                        }
                    }

                    dev_opt =
                        getopt_long(argc, argv, bipString, Options, &index);
                    while (dev_opt != -1 && dev_opt != 'd') {
                        switch (dev_opt) {
                            case 'P':
                                result = atoi(optarg);
                                if (result) {
                                    current->params.bip_params.port =
                                        (uint16_t) result;
                                } else {
                                    current->params.bip_params.port = 0xBAC0;   /* 47808 */
                                }
                                break;
                            case 'n':
                                result = atoi(optarg);
                                if (result) {
                                    current->route_info.net =
                                        (uint16_t) result;
                                } else {
                                    current->route_info.net = port_count;
                                }
                                break;
                        }
                        dev_opt =
                            getopt_long(argc, argv, bipString, Options,
                            &index);
                    }
                    opt = dev_opt;
                } else if (strcmp(optarg, "mstp") == 0) {
                    current->type = MSTP;

                    if (optind < argc && argv[optind][0] != '-') {
                        current->iface = argv[optind];
                    } else {
                        current->iface = "/dev/ttyS0";
                    }

                    /* check if interface is valid */
                    fd = open(current->iface, O_NOCTTY | O_NONBLOCK);
                    if (fd != -1) {
                        close(fd);
                    } else {
                        PRINT(ERROR,
                            "Error: Invalid interface for MSTP device\n");
                        return false;
                    }

                    /* setup default parameters */
                    current->route_info.mac[0] = 127;
                    current->route_info.mac_len = 1;
                    current->params.mstp_params.max_master = 127;
                    current->params.mstp_params.max_frames = 1;
                    current->params.mstp_params.baudrate = 9600;
                    current->params.mstp_params.parity = PARITY_NONE;
                    current->params.mstp_params.databits = 8;
                    current->params.mstp_params.stopbits = 1;
                    current->route_info.net = get_next_free_dnet();

                    dev_opt =
                        getopt_long(argc, argv, mstpString, Options, &index);
                    while (dev_opt != -1 && dev_opt != 'D') {
                        switch (dev_opt) {
                            case 'm':
                                result = atoi(optarg);
                                if (result) {
                                    current->route_info.mac[0] =
                                        (uint8_t) result;
                                }
                                if (argv[optind][0] != '-') {
                                    current->params.mstp_params.max_master =
                                        (uint8_t) atoi(argv[optind]);
                                    if (current->params.mstp_params.
                                        max_master <
                                        current->route_info.mac[0])
                                        current->params.mstp_params.
                                            max_master =
                                            current->route_info.mac[0];

                                    if (argv[optind + 1][0] != '-') {
                                        current->params.mstp_params.
                                            max_frames =
                                            (uint8_t) atoi(argv[optind + 1]);
                                    }
                                }
                                break;
                            case 'b':
                                result = atoi(optarg);
                                if (result) {
                                    current->params.mstp_params.baudrate =
                                        (uint32_t) result;
                                }
                                break;
                            case 'p':
                                switch (optarg[0]) {
                                    case 'E':
                                        current->params.mstp_params.parity =
                                            PARITY_EVEN;
                                        break;
                                    case 'O':
                                        current->params.mstp_params.parity =
                                            PARITY_ODD;
                                        break;
                                    default:
                                        current->params.mstp_params.parity =
                                            PARITY_NONE;
                                        break;
                                }
                                break;
                            case 'd':
                                result = atoi(optarg);
                                if (result >= 5 && result <= 8) {
                                    current->params.mstp_params.databits =
                                        (uint8_t) result;
                                }
                                break;
                            case 's':
                                result = atoi(optarg);
                                if (result >= 1 && result <= 2) {
                                    current->params.mstp_params.stopbits =
                                        (uint8_t) result;
                                }
                                break;
                            case 'n':
                                result = atoi(optarg);
                                if (result) {
                                    current->route_info.net =
                                        (uint16_t) result;
                                }
                                break;
                        }
                        dev_opt =
                            getopt_long(argc, argv, mstpString, Options,
                            &index);
                    }
                    opt = dev_opt;
                } else {
                    PRINT(ERROR, "Error: %s unknown\n", optarg);
                    return false;
                }
                break;
        }
    }
    return true;
}

void init_port_threads(
    ROUTER_PORT * port_list)
{
    ROUTER_PORT *port = port_list;
    pthread_t *thread;

    while (port != NULL) {
        switch (port->type) {
            case BIP:
                port->func = &dl_ip_thread;
                break;
            case MSTP:
                port->func = &dl_mstp_thread;
                break;
        }

        port->state = INIT;
        thread = (pthread_t *) malloc(sizeof(pthread_t));
        pthread_create(thread, NULL, port->func, port);

        pthread_detach(*thread);        /* for proper thread termination */

        port = port->next;
    }
}

bool init_router(
    )
{
    MSGBOX_ID msgboxid;
    ROUTER_PORT *port;

    msgboxid = create_msgbox();
    if (msgboxid == INVALID_MSGBOX_ID)
        return false;

    port = head;
    /* add main message box id to all ports */
    while (port != NULL) {
        port->main_id = msgboxid;
        port = port->next;
    }

    init_port_threads(head);

    /* wait for port initialization */
    port = head;
    while (port != NULL) {
        if (port->state == RUNNING) {
            port = port->next;
            continue;
        } else if (port->state == INIT_FAILED) {
            PRINT(ERROR, "Error: Failed to initialize %s\n", port->iface);
            return false;
        } else {
            PRINT(INFO, "Initializing...\n");
            sleep(1);
            continue;
        }
    }

    return true;
}

void cleanup(
    )
{
    ROUTER_PORT *port;
    BACMSG msg;

    if (head == NULL)
        return;

    msg.origin = head->main_id;
    msg.type = SERVICE;
    msg.subtype = SHUTDOWN;

    del_msgbox(head->main_id);  /* close routers message box */

    /* send shutdown message to all router ports */
    port = head;
    while (port != NULL) {
        if (port->state == RUNNING)
            send_to_msgbox(port->port_id, &msg);
        port = port->next;
    }

    port = head;
    while (port != NULL) {
        if (port->state == FINISHED) {
            cleanup_dnets(port->route_info.dnets);
            port = port->next;
            free(head->iface);
            free(head);
            head = port;
        }
    }

    pthread_mutex_destroy(&msg_lock);
}

void print_msg(
    BACMSG * msg)
{
    if (msg->type == DATA) {
        int i;
        MSG_DATA *data = (MSG_DATA *) msg->data;

        if (data->pdu_len) {
            PRINT(DEBUG, "Message PDU: ");
            for (i = 0; i < data->pdu_len; i++)
                PRINT(DEBUG, "%02X ", data->pdu[i]);
            PRINT(DEBUG, "\n");
        }
    }
}

uint16_t process_msg(
    BACMSG * msg,
    MSG_DATA * data,
    uint8_t ** buff)
{

    BACNET_ADDRESS addr;
    BACNET_NPDU_DATA npdu_data;
    ROUTER_PORT *srcport;
    ROUTER_PORT *destport;
    uint8_t npdu[MAX_NPDU];
    int16_t buff_len = 0;
    int apdu_offset;
    int apdu_len;
    int npdu_len;

    memmove(data, msg->data, sizeof(MSG_DATA));

    apdu_offset = npdu_decode(data->pdu, &data->dest, &addr, &npdu_data);
    apdu_len = data->pdu_len - apdu_offset;

    srcport = find_snet(msg->origin);
    destport = find_dnet(data->dest.net, NULL);
    assert(srcport);

    if (srcport && destport) {
        data->src.net = srcport->route_info.net;

        /* if received from another router save real source address (not other router source address) */
        if (addr.net > 0 && addr.net < BACNET_BROADCAST_NETWORK &&
            data->src.net != addr.net)
            memmove(&data->src, &addr, sizeof(BACNET_ADDRESS));

        /* encode both source and destination for broadcast and router-to-router communication */
        if (data->dest.net == BACNET_BROADCAST_NETWORK ||
            destport->route_info.net != data->dest.net) {
            npdu_len =
                npdu_encode_pdu(npdu, &data->dest, &data->src, &npdu_data);
        } else {
            npdu_len = npdu_encode_pdu(npdu, NULL, &data->src, &npdu_data);
        }

        buff_len = npdu_len + data->pdu_len - apdu_offset;

        *buff = (uint8_t *) malloc(buff_len);
        memmove(*buff, npdu, npdu_len); /* copy newly formed NPDU */
        memmove(*buff + npdu_len, &data->pdu[apdu_offset], apdu_len);   /* copy APDU */

    } else {
        /* request net search */
        return -1;
    }

    /* delete received message */
    free_data((MSG_DATA *) msg->data);

    return buff_len;
}

int kbhit(
    )
{
    static const int STDIN = 0;
    static bool initialized = false;

    if (!initialized) {
        /* use termios to turn off line buffering */
        struct termios term;
        tcgetattr(STDIN, &term);
        term.c_lflag &= ~ICANON;
        tcsetattr(STDIN, TCSANOW, &term);
        setbuf(stdin, NULL);
        initialized = true;
    }

    int bytesWaiting;
    ioctl(STDIN, FIONREAD, &bytesWaiting);
    return bytesWaiting;
}

bool is_network_msg(
    BACMSG * msg)
{

    uint8_t control_byte;       /* NPDU control byte */
    MSG_DATA *data = (MSG_DATA *) msg->data;

    control_byte = data->pdu[1];

    return control_byte & 0x80; /* check 7th bit */
}

uint16_t get_next_free_dnet(
    )
{

    ROUTER_PORT *port = head;
    uint16_t i = 1;
    while (port) {
        if (port->route_info.net == i) {
            port = head;
            i++;
            continue;
        }

        port = port->next;
    }
    return i;
}