/**
 * router.cpp
 *
 * for Vivado HLS
*/

#ifdef SOFTWARE
#include "ap_int.h"
#else
#include <ap_int.h>
#endif

#include "./router.hpp"



static ap_uint<32> lfsr;

void lfsr_random_init(ap_uint<32> seed) {
#pragma HLS INLINE

    lfsr = seed;
}

ap_uint<32> lfsr_random() {
#pragma HLS INLINE

    bool b_32 = lfsr.get_bit(32-32);
    bool b_22 = lfsr.get_bit(32-22);
    bool b_2 = lfsr.get_bit(32-2);
    bool b_1 = lfsr.get_bit(32-1);
    bool new_bit = b_32 ^ b_22 ^ b_2 ^ b_1;
    lfsr = lfsr >> 1;
    lfsr.set_bit(31, new_bit);

    return lfsr.to_uint();
}



// Global values
static ap_uint<7> size_x;    // X
static ap_uint<7> size_y;    // Y
static ap_uint<4> size_z;    // Z

static ap_uint<LINE_BIT> line_num = 0; // #Lines

bool pynqrouter(char boardstr[BOARDSTR_SIZE], char boardstr_high[BOARDSTR_SIZE], ap_uint<32> seed, ap_int<32> *status) {
#pragma HLS INTERFACE s_axilite port=boardstr bundle=AXI4LS
#pragma HLS INTERFACE s_axilite port=boardstr_high bundle=AXI4LS
#pragma HLS INTERFACE s_axilite port=seed bundle=AXI4LS
#pragma HLS INTERFACE s_axilite port=status bundle=AXI4LS
#pragma HLS INTERFACE s_axilite port=return bundle=AXI4LS

    // status(0:Solved, 1:Not solved)
    *status = -1;

    // board
    ap_int<LINE_BIT+1> board[MAX_CELLS];
#pragma HLS ARRAY_PARTITION variable=board cyclic factor=64 dim=1
 // -1024 ~ 1023 (Negative values mean terminals)
    INIT_BOARD_ARRAY:
    for (ap_uint<CELL_BIT> i = 0; i < (ap_uint<CELL_BIT>)(BOARDSTR_SIZE); i++) {
#pragma HLS UNROLL factor=128
        board[i] = 0;
    }


    // ================================
    // (Step.0) Initialization (BEGIN)
    // ================================

    // Note: Loop counter -> need an extra bit (for condition determination)

    /// Parse ///
    size_x = (boardstr[1] - '0') * 10 + (boardstr[2] - '0');
    size_y = (boardstr[4] - '0') * 10 + (boardstr[5] - '0');
    size_z = (boardstr[7] - '0');

    INIT_BOARDS:
    for (ap_uint<CELL_BIT> idx = 8; idx < (ap_uint<CELL_BIT>)(BOARDSTR_SIZE); idx+=11) {

    	// NULL-terminated
        if (boardstr[idx] == 0) break;

        line_num++;

        // Start & Goal of each line
        ap_uint<7> s_x = (boardstr[idx+1] - '0') * 10 + (boardstr[idx+2] - '0');
        ap_uint<7> s_y = (boardstr[idx+3] - '0') * 10 + (boardstr[idx+4] - '0');
        ap_uint<3> s_z = (boardstr[idx+5] - '0') - 1;
        ap_uint<7> g_x = (boardstr[idx+6] - '0') * 10 + (boardstr[idx+7] - '0');
        ap_uint<7> g_y = (boardstr[idx+8] - '0') * 10 + (boardstr[idx+9] - '0');
        ap_uint<3> g_z = (boardstr[idx+10] - '0') - 1;

        ap_uint<CELL_BIT> start_id = (((ap_uint<CELL_BIT>)s_x * MAX_WIDTH + (ap_uint<CELL_BIT>)s_y) << BITWIDTH_Z) | (ap_uint<CELL_BIT>)s_z;
        ap_uint<CELL_BIT> goal_id  = (((ap_uint<CELL_BIT>)g_x * MAX_WIDTH + (ap_uint<CELL_BIT>)g_y) << BITWIDTH_Z) | (ap_uint<CELL_BIT>)g_z;
        board[start_id] = -line_num;
        board[goal_id]  = -line_num;
    }

    // For each line
    ap_uint<LINE_BIT> connected_line_num = 0;
    bool connected[MAX_LINES];
#pragma HLS ARRAY_PARTITION variable=connected cyclic factor=2 dim=1
    INIT_CONNECTED:
    for (ap_uint<LINE_BIT> i = 1; i <= (ap_uint<LINE_BIT>)(line_num); i++) {
#pragma HLS LOOP_TRIPCOUNT min=2 max=20736
#pragma HLS UNROLL factor=4
        connected[i] = false;
    }

    lfsr_random_init(seed);

    // ================================
    // (Step.0) Initialization (END)
    // ================================

	
    // ================================
    // (Step.2) Rip-up Routing (BEGIN)
    // ================================

#ifdef DEBUG_PRINT
    cout << "Routing ..." << endl;
#endif

    bool solved = false;

    ROUTING:
    for (ap_uint<16> round = 0; round < 32768 ; round++) {
#pragma HLS LOOP_TRIPCOUNT min=1 max=32768

        // Target line
        ap_uint<LINE_BIT> target = lfsr_random() % line_num + 1;

#ifdef DEBUG_PRINT
        //cout << "(round " << round << ") LINE #" << (int)target << " " << lfsr_random() << endl;
#endif

        if (connected[target]) continue;

        if (connectable(board, target, 0)) {
            connected[target] = true; connected_line_num++;
        }
        else {
            ap_uint<16> j;
            for (j = 0; j < 10000; j++) {

                // Ripped-up line
                ap_uint<CELL_BIT> rip_up = lfsr_random() % line_num + 1;
                if (connectable(board, target, rip_up)) {
                    connected[target] = true;
                    connected[rip_up] = false;
                    break;
                }
            }
            if (j == 10000) break;
        }
        if (connected_line_num == line_num) {
            solved = true;
            break;
        }
    }

    // Not solved
    if (!solved) {
        *status = 1;  return false;
    }

    // ================================
    // (Step.2) Rip-up Routing (END)
    // ================================


    // ================================
    // (Step.3) Output (BEGIN)
    // ================================

#ifdef DEBUG_PRINT
    cout << "Output ..." << endl;
#endif

    // Init: Blank = 0
    OUTPUT_INIT:
    for (ap_uint<CELL_BIT> i = 0; i < (ap_uint<CELL_BIT>)(MAX_CELLS); i++) {

        if (board[i] < 0) {
            boardstr[i] = (unsigned char)(((-1) * board[i]));
            boardstr_high[i] = (unsigned char)(((-1) * board[i]) >> 8);
        }
        else {
            boardstr[i] = (unsigned char)((board[i]));
            boardstr_high[i] = (unsigned char)((board[i]) >> 8);
        }
    }

    // ================================
    // (Step.3) Output (END)
    // ================================

    *status = 0;  return true;
}


// ================================ //
// For Routing
// ================================ //

bool inside_board(ap_uint<CELL_BIT> cell_id) {
#pragma HLS INLINE

    ap_uint<13> cell_xy = (ap_uint<13>)(cell_id >> BITWIDTH_Z);
    ap_uint<7> cell_x = (ap_uint<7>)(cell_xy / MAX_WIDTH);
    ap_uint<7> cell_y = (ap_uint<7>)(cell_xy - cell_x * MAX_WIDTH);
    ap_uint<3> cell_z = (ap_uint<3>)(cell_id & BITMASK_Z);

    bool ret = false;
    if (cell_x < size_x && cell_y < size_y && cell_z < size_z) {
        ret = true;
    }
    else {
        ret = false;
    }

    return ret;
}

bool connectable(ap_int<LINE_BIT+1> board[MAX_CELLS], ap_uint<LINE_BIT> target, ap_uint<LINE_BIT> rip_up) {

    ap_uint<2> avail[MAX_CELLS];
    ap_uint<CELL_BIT> prev[MAX_CELLS];

    ap_uint<CELL_BIT> start_id = 65535, goal_id = 65535;
    for (ap_uint<CELL_BIT> i = 0; i < (ap_uint<CELL_BIT>)(MAX_CELLS); i++) {

        if (!inside_board(i)) continue;

        prev[i] = 65535; // init.

        if (board[i] == target * (-1)) {
            if (start_id == 65535) {
                start_id = i;
                avail[i] = 3;
            }
            else {
                goal_id = i;
                avail[i] = 0;
            }
        }
        else if (board[i] == 0 || board[i] == rip_up || board[i] == target) {
            avail[i] = 1;
        }
        else {
            avail[i] = 0;
        }
    }

    bool try_connect = available(avail, prev, start_id, goal_id);
    if (try_connect) {
        if (rip_up > 0) {
            remove_line(board, rip_up);
        }
        // Connect line for target
        ap_uint<CELL_BIT> id = prev[goal_id];
        BACKTRACK:
        while (id != start_id) {
#pragma HLS LOOP_TRIPCOUNT min=0 max=256
            board[id] = target; id = prev[id];
        }
    }

    return try_connect;
}

void remove_line(ap_int<LINE_BIT+1> board[MAX_CELLS], ap_uint<LINE_BIT> rip_up) {

    for (ap_uint<CELL_BIT> i = 0; i < (ap_uint<CELL_BIT>)(MAX_CELLS); i++) {
#pragma HLS UNROLL factor=64

        if (!inside_board(i)) continue;
        if (board[i] == rip_up) board[i] = 0;
    }
}

bool available(ap_uint<2> avail[MAX_CELLS], ap_uint<CELL_BIT> prev[MAX_CELLS], ap_uint<CELL_BIT> start_id, ap_uint<CELL_BIT> goal_id) {

    // Priority queue (Circular list)
    ap_uint<PQ_BIT> top = 1, bottom = 0;
    bool is_empty = true;
    ap_uint<CELL_BIT> qu_nodes[MAX_PQ];

    // Point of goal terminal
    ap_uint<13> goal_xy = (ap_uint<13>)(goal_id >> BITWIDTH_Z);
    ap_uint<7> goal_x = (ap_uint<7>)(goal_xy / MAX_WIDTH);
    ap_uint<7> goal_y = (ap_uint<7>)(goal_xy - goal_x * MAX_WIDTH);
    ap_uint<3> goal_z = (ap_uint<3>)(goal_id & BITMASK_Z);

    qu_push(qu_nodes, start_id, &top, &bottom, &is_empty);

    bool complete = false;

    SEARCH_QUEUE:
    while (!is_empty) {
#pragma HLS LOOP_TRIPCOUNT min=1 max=1000
#pragma HLS LOOP_FLATTEN off

        ap_uint<CELL_BIT> src_id; // target cell
        qu_pop(qu_nodes, &src_id, &top, &bottom, &is_empty);


        // End routing
        //if (avail[src_id] == -1) { complete = true; break; } // goal

        if (avail[src_id] != 1 && avail[src_id] != 3) continue; // Keep searching

        // Point of target cell
        ap_uint<13> src_xy = (ap_uint<13>)(src_id >> BITWIDTH_Z);
        ap_uint<7> src_x = (ap_uint<7>)(src_xy / MAX_WIDTH);
        ap_uint<7> src_y = (ap_uint<7>)(src_xy - src_x * MAX_WIDTH);
        ap_uint<3> src_z = (ap_uint<3>)(src_id & BITMASK_Z);
	
        ap_uint<3> isbranch = 0;
        ap_uint<CELL_BIT> tmp_dest_id = 65535;
        // Search adjacent cells (1)
        SEARCH_ADJACENTS_1:
        for (ap_uint<3> a = 0; a < 6; a++) {
            ap_int<8> dest_x = (ap_int<8>)src_x; // Min: -1, Max: 72 (Signed 8bit)
            ap_int<8> dest_y = (ap_int<8>)src_y; // Min: -1, Max: 72 (Signed 8bit)
            ap_int<5> dest_z = (ap_int<5>)src_z; // Min: -1, Max:  8 (Signed 5bit)
            if (a == 0) { dest_x -= 1; }
            if (a == 1) { dest_x += 1; }
            if (a == 2) { dest_y -= 1; }
            if (a == 3) { dest_y += 1; }
            if (a == 4) { dest_z -= 1; }
            if (a == 5) { dest_z += 1; }

            // Inside the board ? //
            if (0 <= dest_x && dest_x < (ap_int<8>)size_x && 0 <= dest_y && dest_y < (ap_int<8>)size_y && 0 <= dest_z && dest_z < (ap_int<5>)size_z) {

                // Adjacent cell
                ap_uint<CELL_BIT> dest_id = (((ap_uint<CELL_BIT>)dest_x * MAX_WIDTH + (ap_uint<CELL_BIT>)dest_y) << BITWIDTH_Z) | (ap_uint<CELL_BIT>)dest_z;
                if (avail[dest_id] >= 2) {
                    isbranch++;
                    tmp_dest_id = dest_id;
                }
            }
        }
        if (isbranch > 1) { avail[src_id] = 0; continue; }
        if (avail[src_id] == 1) {
            avail[src_id] = 2;
            prev[src_id] = tmp_dest_id;
        }

        ap_uint<3> p = 0;
        ap_uint<3> search_order[6];
        if (src_x > goal_x) {
            search_order[p++] = 0; // To src_x--
        }
        else {
            search_order[5+p] = 0;
        }
        if (src_y > goal_y) {
            search_order[p++] = 1; // To src_y--
        }
        else {
            search_order[4+p] = 1;
        }
        if (src_z > goal_z) {
            search_order[p++] = 2; // To src_z--
        }
        else {
            search_order[3+p] = 2;
        }
        if (src_x < goal_x) {
            search_order[p++] = 3; // To src_x++
        }
        else {
            search_order[2+p] = 3;
        }
        if (src_y < goal_y) {
            search_order[p++] = 4; // To src_y++
        }
        else {
            search_order[1+p] = 4;
        }
        if (src_z < goal_z) {
            search_order[p++] = 5; // To src_z++
        }
        else {
            search_order[0+p] = 5;
        }

        ap_uint<3> j, t;
        SHUFFLE_1:
        for (ap_uint<3> a = 0; a < p; a++) {
#pragma HLS LOOP_TRIPCOUNT min=1 max=3
            j = lfsr_random() % p;
            t = search_order[a];
            search_order[a] = search_order[j];
            search_order[j] = t;
        }
        SHUFFLE_2:
        for (ap_uint<3> a = p; a < 6; a++) {
#pragma HLS LOOP_TRIPCOUNT min=3 max=5
            j = lfsr_random() % (6-p) + p;
            t = search_order[a];
            search_order[a] = search_order[j];
            search_order[j] = t;
        }

        //cout << "(" << src_x << ", " << src_y << ", " << src_z << ")->";
        //cout << "(" << goal_x << ", " << goal_y << ", " << goal_z << "): p = " << p << " [";
        //for (ap_uint<3> a = 0; a < 6; a++) {
        //    cout << search_order[a];
        //    if (a != 5) cout << " ";
        //}
        //cout << "]" << endl;
        
        // Search adjacent cells (2)
        SEARCH_ADJACENTS_2:
        for (ap_uint<3> a = 0; a < 6; a++) {
            ap_int<8> dest_x = (ap_int<8>)src_x; // Min: -1, Max: 72 (Signed 8bit)
            ap_int<8> dest_y = (ap_int<8>)src_y; // Min: -1, Max: 72 (Signed 8bit)
            ap_int<5> dest_z = (ap_int<5>)src_z; // Min: -1, Max:  8 (Signed 5bit)
            if (search_order[5-a] == 0) { dest_x -= 1; }
            if (search_order[5-a] == 1) { dest_y -= 1; }
            if (search_order[5-a] == 2) { dest_z -= 1; }
            if (search_order[5-a] == 3) { dest_x += 1; }
            if (search_order[5-a] == 4) { dest_y += 1; }
            if (search_order[5-a] == 5) { dest_z += 1; }

            // Inside the board ? //
            if (0 <= dest_x && dest_x < (ap_int<8>)size_x && 0 <= dest_y && dest_y < (ap_int<8>)size_y && 0 <= dest_z && dest_z < (ap_int<5>)size_z) {

                // Adjacent cell
                ap_uint<CELL_BIT> dest_id = (((ap_uint<CELL_BIT>)dest_x * MAX_WIDTH + (ap_uint<CELL_BIT>)dest_y) << BITWIDTH_Z) | (ap_uint<CELL_BIT>)dest_z;
                if (dest_id == goal_id) {
                    prev[dest_id] = src_id;
                    complete = true;
                }
                qu_push(qu_nodes, dest_id, &top, &bottom, &is_empty);
            }
        }
        if (complete) break;
    }

    return complete;
}

// Queue push (Enqueue)
// First In Last Out
void qu_push(ap_uint<CELL_BIT> qu_nodes[MAX_PQ], ap_uint<CELL_BIT> id, ap_uint<PQ_BIT> *top, ap_uint<PQ_BIT> *bottom, bool *is_empty) {
#pragma HLS INLINE

    (*bottom)++;
    if ((*bottom) == (*top) && !(*is_empty)) { (*top)++; } // Queue is full -> First element is automatically removed
    qu_nodes[(*bottom)] = id;
    *is_empty = false;
}

// Queue pop (Dequeue)
// First In Last Out
void qu_pop(ap_uint<CELL_BIT> qu_nodes[MAX_PQ], ap_uint<CELL_BIT> *id, ap_uint<PQ_BIT> *top, ap_uint<PQ_BIT> *bottom, bool *is_empty) {
#pragma HLS INLINE

    *id = qu_nodes[(*bottom)];
    (*bottom)--;
    if (((*bottom)-(*top)+1) == 0) { *is_empty = true; }
}