#include 
#include 
#include 
#include "sa.h"
 
#define PRINT(S) for(i = 0; i < N_OBJ; i++) \
                         printf("%d ", S[i]); \
                         printf("\n");
#define RANDOM() ((double)rand() / (double)RAND_MAX)
 
int nb[N_OBJ];
double T = 2000;
double rad = 0.9;
 
char _test(int * bag, int n) {
    int i;
    double w = 0;
    double v = 0;
    for (i = 0; i < n; i++) {
        w += objects[bag[i] - 1].weight;
        v += objects[bag[i] - 1].volume;
    }
    return w <= WEIGHT && v <= WEIGHT;
}
 
double _dispatch(int * bag, int n) {
    int i, loc = 0;
    double distance = 0;
    for (i = 0; i < n; i++) {
        int next = objects[bag[i] - 1].loc;
        distance += dismat[loc][next];
        loc = next;
    }
    distance += dismat[loc][0];
    return distance;
}
 
/* l should go back one step if
 * it doesn't pass the test.
 */
double E(int * S) {
    int p = 0;
    double distance = 0;
    do {
        int l = p;
        while (l < N_OBJ && _test(S + p, l - p))
            l++;
        if (l < N_OBJ || !_test(S + p, l - p))
            l--;
        distance += _dispatch(S + p, l - p);
        p = l;
    } while (p < N_OBJ);
    return distance;
}
 
char accept(double eo, double en, double t) {
    if (en < eo)
        return 1;
    else
        return exp(-(en - eo) / t) > RANDOM();
}
 
void neighbour(int * S) {
    int i, j, h;
    i = rand() % N_OBJ;
    j = rand() % N_OBJ;
    if (i > j) {
        h = i;
        i = j;
        j = h;
    }
    for (h = 0; h < i; h++)
        nb[h] = S[h];
    for (h = j + 1; h < N_OBJ; h++)
        nb[h] = S[h];
    while (i <= j) {
        nb[i] = S[j], nb[j] = S[i];
        i++; j--;
    }
}
 
void copy(int * new_state, int * state) {
    int i;
    for (i = 0; i < N_OBJ; i++)
        state[i] = new_state[i];
}
 
int main(int argc, char * argv[]) {
    int i;
    int state[N_OBJ];
    int best_state[N_OBJ];
    double eo, en, ebest = 1e10;
    for (i = 0; i < N_OBJ; i++) 
        state[i] = i + 1;
    eo = E(state);
    srand((unsigned)time(0));
    while (T > 0.1) {
        for (i = 0; i < 50000; i++) {
            neighbour(state);
            en = E(nb);
            if (accept(eo, en, T)) {
                eo = en;
                copy(nb, state);
                if (eo < ebest) {
                    copy(state, best_state);
                    ebest = eo;
                }
            }
        }
        T *= rad;
        printf("%lf\t%lf\n", T, eo);
    }
    printf("%lf\n", ebest / 24000 + 5);
    PRINT(best_state);
    return 0;
}