/* genetic_algorithm.c -- This belongs to gneural_network

   gneural_network is the GNU package which implements a programmable neural network.

   Copyright (C) 2016 Jean Michel Sellier
   <address@hidden>

   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, 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/>.

*/

// performs a genetic search of the best weights during the training process

// initially implemented by : Jean Michel Sellier
// drastically improved by  : Nan

#include "genetic_algorithm.h"

#ifdef _OPENMP
#include <omp.h>

#define PAR_QSORT_LOW_LIMIT 1024

void memswap(void* a, void* b, size_t size) {
    // suppose a and b never overlapped.
    if (a == b) return;
    void* t = malloc(size);
    memcpy(t, a, size);
    memcpy(a, b, size);
    memcpy(b, t, size);
    free(t);
}

void* partition(void* data, size_t element_count, size_t element_size, __compar_fn_t cmp_func) {
    void* pivot = data + (element_count - 1) * element_size;
    void* i = data;
    void* j = data;
    while (j < pivot) {
        if (cmp_func(j, pivot) < 0) {
            memswap(i, j, element_size);
            i += element_size;
        }
        j += element_size;
    }
    memswap(i, pivot, element_size);
    return i;
}

void par_qsort(void* data, size_t element_count, size_t element_size, __compar_fn_t cmp_func) {
    if (element_count < PAR_QSORT_LOW_LIMIT) {
        qsort(data, element_count, element_size, cmp_func);
    } else {
        void* pivot = partition(data, element_count, element_size, cmp_func);
        size_t left = (pivot - data) / element_size;
        size_t right = element_count - left - 1;
        #pragma omp parallel sections firstprivate(data, element_count, element_size, cmp_func, pivot, left, right)
        {
            #pragma omp section
            par_qsort(data, left, element_size, cmp_func);
            #pragma omp section
            par_qsort(pivot + element_size, right, element_size, cmp_func);
        }

    }
}
#endif

typedef struct {
    double error;
    double* weights;
} individual_t;

int actual_weight_count(void) {
    int k = 0;
    int i, j;
    for (i = 0; i < NNUM; ++i) {
        for (j = 0; j < NEURON[i].nw; ++j) {
            k++;
        }
    }
    return k;
}

void crossover(double w1, double w2, double* n1, double* n2) {
    static int POSS = 1;
    double average = (w1 + w2) / 2;
    double mid = (WMAX + WMIN) / 2;
    *n1 = average;
    if (average > mid) {
        *n2 = average - (WMAX - WMIN) / 2;
    } else if (average < mid) {
        *n2 = average + (WMAX - WMIN) / 2;
    } else { /* == mid*/
        if (POSS) {
            *n2 = WMAX;
            POSS = 0;
        } else {
            *n2 = WMIN;
            POSS = 1;
        }
    }
}

void mutation(double* weight, double rate) {
    if (rnd() > rate) return;
    if (rnd() > 0.5) {
        /* go plus */
        *weight += (rnd() * (WMAX - WMIN) / 2);
        if (*weight > WMAX) *weight -= (WMAX - WMIN);
    } else {
        /* go minus */
        *weight -= (rnd() * (WMAX - WMIN) / 2);
        if (*weight < WMIN) *weight += (WMAX - WMIN);
    }
}

int individual_compare(const void* a, const void* b) {
    individual_t* ia = *(individual_t**)a;
    individual_t* ib = *(individual_t**)b;
    if (ia->error > ib->error) return 1;
    if (ia->error < ib->error) return -1;
    return 0;
}

void init_individuals(individual_t** individuals, int size) {
    int n;
#ifdef _OPENMP
    #pragma omp parallel for shared(NNUM, NEURON, individuals) private(n)
#endif
    for (n = 0; n < size; ++n) {
        int k = 0;
        int i, j;
        for (i = 0; i < NNUM; i++) {
            for (j = 0; j < NEURON[i].nw; j++) {
                individuals[n]->weights[k] = rnd();
                k++;
            }
        }
    }
}

void reproduce_next_generation(individual_t** individuals, int size, int weight_cout, double rate) {
    int pos = size;
    int i, j, k;
#ifdef _OPENMP
    #pragma omp parallel for shared(pos, individuals, size, weight_cout, rate) private(i, j, k)
#endif
    for (i = 0; i < size; ++i) {
        for (j = i + 1; j < size; ++j) {
            for (k = 0; k < weight_cout; ++k) {
                double w1 = individuals[i]->weights[k];
                double w2 = individuals[j]->weights[k];
                crossover(w1, w2, individuals[pos]->weights + k, individuals[pos + 1]->weights + k);
                mutation(individuals[pos]->weights + k, rate);
                mutation(individuals[pos + 1]->weights + k, rate);
            }
#ifdef _OPENMP
            #pragma omp atomic
#endif
            pos += 2;
        }
    }
}

void selection(individual_t** individuals, int size) {
    int pool_size = size * size;
    int n;
#ifdef _OPENMP
    // issue here because network and neuron are both global here, and even use
    // firstprivate cannot make local copy correctly. without parallelized error
    // calculation, everything goes correct.
    // in future, we might need a clone_network method to make a local copy of
    // network OR make a Neuron ownership belongs to network
    #pragma omp parallel for shared(individuals, NEURON, NETWORK) private(n)
    //#pragma omp parallel for shared(individuals) private(n) firstprivate(NEURON, NETWORK)
#endif
    for (n = 0; n < pool_size; ++n) {
        #pragma omp critical
        {
            int k = 0;
            int i, j;
            for (i = 0; i < NNUM; i++) {
                for (j = 0; j < NEURON[i].nw; j++) {
                    NEURON[i].w[j] = individuals[n]->weights[k];
                    k++;
                }
            }
            individuals[n]->error = error(ERROR_TYPE);
        }
    }
#ifdef _OPENMP
    par_qsort(individuals, pool_size, sizeof(individual_t*), individual_compare);
#else
    qsort(individuals, pool_size, sizeof(individual_t*), individual_compare);
#endif
}

void genetic_algorithm(int output /* screen output - on/off */,
                       int nmax /* number of generations */,
                       int npop /* number of individuals per generation */,
                       double rate /* rate of change between one generation and the parent */,
                       double eps /* numerical accuracy */) {
    int i, j, k, n;

    int pool_size = npop * npop;
    individual_t** individuals = malloc(pool_size * sizeof(individual_t*));
    if (individuals == NULL) {
        printf("GA: Not enough memory to allocate individual index table\n");
        exit(0);
    }

    for (i = 0; i < pool_size; ++i) {
        individuals[i] = malloc(sizeof(individual_t));
        if (individuals[i] == NULL) {
            printf("GA: Not enough memory to allocate individuals\n");
            exit(0);
        }
        individuals[i]->weights = malloc(NNUM * MAX_IN * sizeof(double));
        if (individuals[i]->weights == NULL) {
            printf("GA: Not enough memory to allocate weights\n");
            exit(0);
        }
    }
    init_individuals(individuals, npop);
    int weight_cout = actual_weight_count();

    for (n = 0; n < nmax; ++n) {
        reproduce_next_generation(individuals, npop, weight_cout, rate);
        selection(individuals, npop);
        if (output == ON) printf("GA2: %d %.12g\n", n, individuals[0]->error);
        if (individuals[0]->error < eps) break;
    }

    if (individuals[0]->error > eps) {
        if (output == ON) printf("GA2: error still greater than %g after %d iterations", eps, nmax);
    }

    k = 0;
    for (i = 0; i < NNUM; i++) {
        for (j = 0; j < NEURON[i].nw; j++) {
            NEURON[i].w[j] = individuals[0]->weights[k];
            k++;
        }
    }

    for (i = 0; i < pool_size; ++i) {
        free(individuals[i]->weights);
        free(individuals[i]);
    }
    free(individuals);
}