// Project Identifier: 9B734EC0C043C5A836EA0EBE4BEFEA164490B2C7
#include <map>
#include <cmath>
#include <vector>
#include <string>
#include <limits>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <getopt.h>
#include <algorithm>
#include <unordered_map>
#include "TSP.h"

// UTILITIES //////////////////////////////////////////////////////////////////////////////
// set unvisited
void set_unvisited(OPT &opt){
    for(size_t i=0; i < opt.path.size(); i++){
        opt.path[i].visited = false;
    }
}

// set visited
void set_visited(std::vector<Vertex> &container, size_t &permLength){
    for(size_t i=0; i < permLength; i++){
        container[i].visited = true;
    }
}
// get vertices
void get_vertices(std::vector<Vertex> &container){
    size_t num = 0;
    std::cin >> num;
    // for num locations, cin location and add to vector
    for(size_t i=0; i < num; i++){
        double x, y;
        std:: cin >> x >> y;
        Vertex new_vertex = Vertex(i, x, y);
        container.push_back(new_vertex);
    }
}

// returns weight from two vertices
double find_weight_v(Vertex &vertex_a, Vertex &vertex_b){
    return find_distance(vertex_a.x, vertex_a.y, vertex_b.x, vertex_b.y);
}

//add up all edges to get path length
double calc_path_length(std::vector<Vertex> &container){
    double path_length = 0;
    for(size_t i=1; i < container.size(); i++){
        path_length += find_weight_v(container[i-1], container[i]);
    }
    return path_length;
}

// returns edge weight (dik + dkj - dij)
double calc_edge(Vertex &vertex, std::vector<Vertex> &path, size_t &index){
    return find_weight_v(path[index], vertex) + find_weight_v(vertex, path[index + 1]) - find_weight_v(path[index], path[index + 1]);
}

// returns index of path with minimal length
size_t min_path(std::vector<std::vector<Vertex> > &perm_container){
    double min_length = std::numeric_limits<double>::infinity();
    size_t min_index = 0;
    for(size_t i=0; i < perm_container.size(); i++){
        if(calc_path_length(perm_container[i]) < min_length){
            min_length = calc_path_length(perm_container[i]);
            min_index = i;
        }
    }
    return min_index;
}

// PART B ////////////////////////////////////////////////////////////////////////////////
// finds nearest location to last inserted element in path
size_t nearest_location(size_t &last_inserted, std::vector<Vertex> &path, std::vector<Vertex> &container){
    // initialize nearest info
    double nearest_weight = std::numeric_limits<double>::infinity();
    size_t nearest_index = 0;
    // find nearest non-inserted location in container
    for(size_t i=0; i < container.size(); i++){
        if(!container[i].visited){
            // find closest location in container to last inserted location in path
            if(find_weight_v(path[last_inserted], container[i]) < nearest_weight){
                nearest_weight = find_weight_v(path[last_inserted], container[i]);
                nearest_index = i;
            }
        }
    }
    return nearest_index;
}

// returns insertion index that would cause the smallest increase in path length
size_t insert_position(std::vector<Vertex> &path, Vertex &vertex){
    double min_weight = std::numeric_limits<double>::infinity();
    size_t insert_index = 0;
    for(size_t i=0; i < path.size() - 1; i++){
        if(calc_edge(vertex, path, i) < min_weight){
            min_weight = calc_edge(vertex, path, i);
            insert_index = i + 1;
        }
    }
    return insert_index;
}

// run fasttsp
void run_fasttsp(std::vector<Vertex> &container){
    // initialize path, index
    std::vector<Vertex> path;
    size_t index = 0;
    // add origin start/end to container
    size_t insertion_count = 2;
    container[index].visited = true;
    path.push_back(container[index]);
    path.push_back(container[index]);
    size_t last_inserted = 0;

    // while number inserted is less than number of vertices
    while(insertion_count < container.size() + 1){
        size_t to_insert = nearest_location(last_inserted, path, container);
        size_t insert_index = insert_position(path, container[to_insert]);
        path.insert(path.begin() + (int)insert_index, container[to_insert]);
        container[to_insert].visited = true;
        last_inserted = insert_index;
        insertion_count++;
    }
    std::cout << calc_path_length(path) << "\n";
    for(size_t i=0; i < path.size() - 1; i++){
        std::cout << path[i].id << " ";
    }
}

// PART C ////////////////////////////////////////////////////////////////////////////////////////////
// DONE create distance matrix
void construct_matrix(std::vector<Vertex> &container, std::vector<std::vector<double> > &distance_matrix){
    // create container in order according to vertex id
    std::sort(container.begin(), container.end());
    for(size_t i=0; i < container.size(); i++){
        // for each vertex, create vector
        std::vector<double> distances;
        // fill vector with distances(in order)
        for(size_t j=0; j < container.size(); j++){
            distances.push_back(find_weight_v(container[i], container[j]));
        }
        distance_matrix.push_back(distances);
    }
}

// DONE get upper bound info from FASTTSP calculation
void get_upper_bound(OPT &opt){
    // FASTTSP (Greedy Algroithm)
    size_t index = 0;
    size_t insertion_count = 2;
    opt.container[index].visited = true;
    opt.best_path.push_back(opt.container[index]);
    opt.best_path.push_back(opt.container[index]);
    size_t last_inserted = 0;
    while(insertion_count < opt.container.size() + 1){
        size_t to_insert = nearest_location(last_inserted, opt.best_path, opt.container);
        size_t insert_index = insert_position(opt.best_path, opt.container[to_insert]);
        opt.best_path.insert(opt.best_path.begin() + (int)insert_index, opt.container[to_insert]);
        opt.container[to_insert].visited = true;
        last_inserted = insert_index;
        insertion_count++;
    }
    // get distance
    opt.best_distance = calc_path_length(opt.best_path);
    // get path
    /*
    for(size_t i=0; i < opt.path.size() - 1; i++){
        opt.best_path.push_back(opt.path[i]);
    }
    */
    //print_path(opt.path);
}

// construct mst for gen_perms
double construct_mst(OPT &opt, size_t permLength){
    // set "visited"
    //set_visited(container, permLength);
    // set beginning
    double lower_bound = 0;
    
    opt.path[permLength].distance = 0;
    opt.path[permLength].prev = 0;
    
    // start loop
    for(size_t i=permLength; i < opt.path.size(); i++){
        size_t min_index = permLength;
        double min_weight = std::numeric_limits<double>::infinity();
        for(size_t j=permLength; j < opt.path.size(); j++){
            if(!opt.path[j].visited){
                if(opt.path[j].distance < min_weight){
                    min_weight = opt.path[j].distance;
                    min_index = j;
                }
            }
        }
        opt.path[min_index].visited = true;
        lower_bound += opt.path[min_index].distance;
        for(size_t j=permLength; j < opt.path.size(); j++){
            if(!opt.path[j].visited){
                //weight = find_weight(container[min_index], container[j]);
                min_weight = opt.distance_matrix[opt.path[j].id][opt.path[min_index].id];
                if(min_weight < opt.path[j].distance){
                    opt.path[j].distance = min_weight;
                    opt.path[j].prev = opt.path[min_index].id;
                }
            }
        }
    }
    return lower_bound;
}

// PROMISING
bool promising(OPT &opt, size_t permLength){
    // bool promise = false;
    // pruning check
    if(opt.path.size() - permLength < static_cast<size_t>(5)){
        return true;
    }
    
    // get lower bound from mst
    double lower_bound = construct_mst(opt, permLength);

    // pruning check
    if(lower_bound + opt.current_distance > opt.best_distance){
        return false;
    }

    // initialize arms
    double arm_a = opt.distance_matrix[opt.path[0].id][opt.path[permLength].id];
    double arm_b = opt.distance_matrix[opt.path[permLength - 1].id][opt.path[permLength].id];

    // set arms
    for(size_t i = permLength + 1; i < opt.path.size(); i++){
        if(opt.distance_matrix[opt.path[0].id][opt.path[i].id] < arm_a)
            arm_a = opt.distance_matrix[opt.path[0].id][opt.path[i].id];
        if(opt.distance_matrix[opt.path[permLength - 1].id][opt.path[i].id] < arm_b)
            arm_b = opt.distance_matrix[opt.path[permLength - 1].id][opt.path[i].id];
    }

    // lower bound + arms + current distance
    if(lower_bound + arm_a + arm_b + opt.current_distance < opt.best_distance){
        return true;
    }
    /*
    // DEBUG
    for (size_t i = 0; i < opt.path.size(); ++i)
        std::cerr << std::setw(2) << opt.path[i].id << ' ';
        std::cerr << std::setw(4) << permLength << std::setw(10) << opt.current_distance;
        std::cerr << std::setw(10) << arm_a << std::setw(10) << arm_b;
        std::cerr << std::setw(10) << lower_bound << std::setw(10) << (lower_bound + arm_a + arm_b + opt.current_distance) << "  " << promise << '\n';
    ////
    //return promise
    */
   return false;
}


// part c fixes part a and checks to see if its a good start
// gen perms
// call gen perms with perm length 
// first call is gen perms(1)
void gen_perms(OPT &opt, size_t permLength) {
    //print_path(opt.path);
    //std::cout << "genning perms\n";
    // base case, add an edge from last vertex in path back to origin
    if (permLength == opt.path.size()) {
        //std::cout << "base case!\n";
        // Do something with the path
        double edge = opt.distance_matrix[opt.path[permLength - 1].id][opt.path[0].id];
        if(opt.current_distance + edge <= opt.best_distance){
            opt.best_distance = opt.current_distance + edge;
            reassign_path(opt);
        }
        return;
    }

    // compute lower bounds on the length of any full tour that can be found in a given branch. 
    // If such a lower bound exceeds the cost of a full solution you have found previously, you can skip this branch as hopeless.
    if (!promising(opt, permLength)) {
        //std::cout << "not promising\n";
        return;
    }
    set_unvisited(opt);

    for (size_t i = permLength; i < opt.path.size(); ++i) {
        std::swap(opt.path[permLength], opt.path[i]);
        // try adding a new vertex to the path
        opt.current_distance += opt.distance_matrix[opt.path[permLength].id][opt.path[permLength - 1].id];
        gen_perms(opt, permLength + 1);
        opt.current_distance -= opt.distance_matrix[opt.path[permLength].id][opt.path[permLength - 1].id];
        std::swap(opt.path[permLength], opt.path[i]);
    }
}


//run opttsp
void run_opttsp(OPT &opt){
    // container is initial set of vertices
    // initialize distance matrix 
    construct_matrix(opt.container, opt.distance_matrix);
    init_path(opt);
    // get upper bound from FASTTSP
    get_upper_bound(opt);
    //std::cout << "upper bound: ";
    //print_path(opt.best_path);
    set_unvisited(opt);
    
    opt.best_path.pop_back();
    // Call genPerms(1)
    gen_perms(opt, static_cast<size_t>(1));
    //opt.best_path = opt.path;
    // output
    //std::cout << current_distance << "\n";
    std::cout << opt.best_distance << "\n";
    for(size_t i=0; i < opt.best_path.size(); i++){
        std::cout << opt.best_path[i].id << " ";
    }
}



////////////////////////////////////////////////////////////////////
// TESTING /////////////////////////////////////////////////////////
void init_path(OPT &opt){
    for(size_t i=0; i < opt.container.size(); i++){
        opt.path.push_back(opt.container[i]);
    }
}
void reassign_path(OPT &opt){
    opt.best_path.clear();
    for(size_t i=0; i < opt.path.size(); i++){
        opt.best_path.push_back(opt.path[i]);
    }
}
// print path for testing
void print_path(std::vector<Vertex> &path){
for(size_t i=0; i < path.size(); i++){
        std::cout << path[i].id << " ";
    }
    std::cout << "\n";
}
// test container
void test_perm_container(std::vector<std::vector<Vertex> > &perm_container){
    for(size_t i = 0; i < perm_container.size(); i++){
        for(size_t j = 0; j < perm_container[i].size(); j++){
            std::cout << perm_container[i][j].id << " ";
        }
        std::cout << "\n";
        std::cout << "Path length: " << calc_path_length(perm_container[i]) << "\n";
    }
}

//gen perms
void test_perms(std::vector<std::vector<Vertex> > &perm_container, std::vector<Vertex> &v_container, size_t permLength, size_t h){
    if(permLength == h) {
        perm_container.push_back(v_container);
        return;
  }
    //if(!promising(path, permLength))
        //return;
    for(size_t i = permLength; i < h; i++) {
        std::swap(v_container[permLength], v_container[i]);
        test_perms(perm_container, v_container, permLength + 1, h);
        std::swap(v_container[permLength], v_container[i]);
    } 
} 

//run opttsp
void test_run_opttsp(std::vector<Vertex> &v_container){
    std::vector<std::vector<Vertex> > perm_container;
    v_container.push_back(v_container[0]);

    //generate all permutations
    //size_t num_vertices = v_container.size();
    size_t init_gen = 1;
    size_t x = v_container.size() - 1;

    //start with brute force approach
    test_perms(perm_container, v_container, init_gen, x);

    //find optimal path
    double min_length = std::numeric_limits<double>::infinity();
    size_t min_index = 0;
    for(size_t i=0; i < perm_container.size(); i++){
        if(calc_path_length(perm_container[i]) < min_length){
            min_length = calc_path_length(perm_container[i]);
            min_index = i;
        }
    }
    
    //output
    std::cout << min_length << "\n";
    for(size_t i=0; i < perm_container[min_index].size() - 1; i++){
        std::cout << perm_container[min_index][i].id << " ";
    }
    std::cout << "\n";
}