// TODO: These are copied from the old Population.cpp. Should
// re-implement properly!

// -------------------------------------------------------------------
// TODO: Reimplement functions without global.h and clist.h
/* A custom doubly linked list implemenation */

# include <cstdio>
# include <cstdlib>
# include <cmath>

#include<algorithm>
#include<random>

#include<iostream>

#include "Population.hpp"

// ---------------- place properly
namespace Rnd{

/* (Directly from Deb's code) Fetch a single random integer between low and high including the bounds */
int
RndIntBetween(int low, int high, mt19937 *mt){
    std::uniform_real_distribution<double> uniform01(0.,1.);
    int res;
    if (low >= high){
        res = low;
    }
    else {
        res = low + (uniform01(*mt)*(high-low+1));
        if (res > high) {
            res = high;
        }
    }
    return (res);
}

}



typedef struct lists
{
    int index;
    struct lists *parent;
    struct lists *child;
}
list;

/* Insert an element X into the list at location specified by NODE */
static
void insert (list *node, int x)
{
    list *temp;
    if (node==NULL)
    {
        printf("\n Error!! asked to enter after a NULL pointer, hence exiting \n");
        exit(1);
    }
    temp = (list *)malloc(sizeof(list));
    temp->index = x;
    temp->child = node->child;
    temp->parent = node;
    if (node->child != NULL)
    {
        node->child->parent = temp;
    }
    node->child = temp;
    return;
}

/* Delete the node NODE from the list */
static
list* del (list *node)
{
    list *temp;
    if (node==NULL)
    {
        printf("\n Error!! asked to delete a NULL pointer, hence exiting \n");
        exit(1);
    }
    temp = node->parent;
    temp->child = node->child;
    if (temp->child!=NULL)
    {
        temp->child->parent = temp;
    }
    free (node);
    return (temp);
}


#define INF 1.0e14

// -------------------------------------------------------------------

int
Population::binaryTournament (int a, int b, mt19937 *mt) const
{
    return (_individuals[a]->winsTournamentAgainst(*_individuals[b], mt))?a:b;
}


// Dummy placeholder function for select&cross
// TODO: Imitate specifics from Deb's code.
void
Population::selectAndCrossFrom(Population &parent, std::mt19937* mt)
{
    int *a1, *a2;
    int temp;
    int i;
    int rand;
    a1 = (int *)malloc(size()*sizeof(int));
    a2 = (int *)malloc(size()*sizeof(int));

    // Create two random permutations of indices:
    for (i=0; i<size(); i++)
    {
        a1[i] = a2[i] = i;
    }
    for (i=0; i<size(); i++)
    {
        rand = Rnd::RndIntBetween(i, size()-1, mt);
        temp = a1[rand];
        a1[rand] = a1[i];
        a1[i] = temp;
        rand = Rnd::RndIntBetween(i, size()-1, mt);
        temp = a2[rand];
        a2[rand] = a2[i];
        a2[i] = temp;
    }

    // Use the permutations to create children
    for (i=0; i<size(); i+=4)
    {
        /* 
        parent1 = tournament (&old_pop->ind[a1[i]], &old_pop->ind[a1[i+1]]);
        parent2 = tournament (&old_pop->ind[a1[i+2]], &old_pop->ind[a1[i+3]]);
        crossover (parent1, parent2, &new_pop->ind[i], &new_pop->ind[i+1]);
        parent1 = tournament (&old_pop->ind[a2[i]], &old_pop->ind[a2[i+1]]);
        parent2 = tournament (&old_pop->ind[a2[i+2]], &old_pop->ind[a2[i+3]]);
        crossover (parent1, parent2, &new_pop->ind[i+2], &new_pop->ind[i+3]);
        */

        // Using our interfaces:
        int ipar1, ipar2;
        ipar1 = parent.binaryTournament(a1[i],a1[i+1],mt);
        ipar2 = parent.binaryTournament(a1[i+2],a1[i+3],mt);
        auto twins = parent._individuals[ipar1]->crossWith(*(parent._individuals[ipar2]));
        _individuals[i] = move(twins.first);
        _individuals[i+1] = move(twins.second);

        ipar1 = parent.binaryTournament(a2[i],a2[i+1],mt);
        ipar2 = parent.binaryTournament(a2[i+2],a2[i+3],mt);
        twins = parent._individuals[ipar1]->crossWith(*(parent._individuals[ipar2]));
        _individuals[i+2] = move(twins.first);
        _individuals[i+3] = move(twins.second);

    }

    free (a1);
    free (a2);


}


/*  (Interface directly from Deb's code) Sort front according to objective */
void Population::quicksortFrontObj(int objcount, int* obj_array, int front_size)
const
{
    // Our task is to sort the index array according to one
    // objective's value in each individual.

    vector<int> inds(front_size);
    for(size_t k=0;k<inds.size();++k){inds[k]=obj_array[k];}

    sort(inds.begin(), inds.end(), 
             [&](const int & a, const int & b)
    {
        double obja = _individuals[a]->getObjective(objcount);
        double objb = _individuals[b]->getObjective(objcount);
        bool result = (obja < objb);
        return result;
    });
    for(size_t k=0;k<inds.size();++k){obj_array[k]=inds[k];}
}

/*  (Interface directly from Deb's code) Sort front according to crowding distance */
void Population::quicksortDist(int *dist, int front_size) const
{
    // TODO: Have the structure as vector& in the first place..
    vector<int> inds(front_size);
    for (int i = 0; i < (int)inds.size(); ++i)
    {
        inds[i] = dist[i];
    }
    sort(inds.begin(), inds.end(),
         [&](const int & a, const int & b) -> bool
         {
            double dista = _individuals[a]->getCrowdingDistance();
            double distb = _individuals[b]->getCrowdingDistance();
            bool result = (dista < distb);
            return result;
         });
    for (int i = 0; i < (int)inds.size(); ++i)
    {
        dist[i] = inds[i];
    }
}



/* (Directly from Deb's code) Routine to fill a population with individuals in the decreasing order of crowding distance */
void Population::crowdingFillFrom(Population &mixed, int count, int front_size, void *plstelite)
{
    int *dist;
    list *temp;
    int i, j;
    list *elite;
    elite = (list*) plstelite;
    mixed.assignCrowdingDistanceList(elite->child, front_size);

    dist = (int *)malloc(front_size*sizeof(int));
    temp = elite->child;
    for (j=0; j<front_size; j++)
    {
        dist[j] = temp->index;
        temp = temp->child;
    }
    // Our indexes are into the mixed population:
    mixed.quicksortDist(dist, front_size);
    for (i=count, j=front_size-1; i< mixed.size()/2; i++, j--)
    {
        //copy_ind(&mixed_pop->ind[dist[j]], &new_pop->ind[i]);
        _individuals[i] = move(mixed._individuals[dist[j]]);
    }
    free (dist);
    return;
}

/*  (Directly from Deb's code) Routine to compute crowding distances */
void Population::assignCrowdingDistance (int *dist, int **obj_array, int front_size)
{
    int i, j;
    
    for (i=0; i<_nobj; i++)
    {
        for (j=0; j<front_size; j++)
        {
            obj_array[i][j] = dist[j];
        }
        quicksortFrontObj(i, obj_array[i], front_size);
    }
    for (j=0; j<front_size; j++)
    {
        _individuals[dist[j]]->setCrowdingDistance(0.0);
    }
    for (i=0; i<_nobj; i++)
    {
        _individuals[obj_array[i][0]]->setCrowdingDistance(INF);
    }
    for (i=0; i<_nobj; i++)
    {
        for (j=1; j<front_size-1; j++)
        {
            if (_individuals[obj_array[i][j]]->getCrowdingDistance() != INF)
            {
                if (_individuals[obj_array[i][front_size-1]]->getObjective(i) == _individuals[obj_array[i][0]]->getObjective(i))
                {
                    _individuals[obj_array[i][j]]->setCrowdingDistance(
                        _individuals[obj_array[i][j]]->getCrowdingDistance() 
                        + 0.0
                    );
                }
                else
                {
                    _individuals[obj_array[i][j]]->setCrowdingDistance(
                        _individuals[obj_array[i][j]]->getCrowdingDistance() 
                        + (  _individuals[obj_array[i][j+1]]->getObjective(i) 
                           - _individuals[obj_array[i][j-1]]->getObjective(i)
                          ) 
                        / 
                          (_individuals[obj_array[i][front_size-1]]->getObjective(i) 
                           - _individuals[obj_array[i][0]]->getObjective(i)
                          )
                        );
                }
            }
        }
    }
    for (j=0; j<front_size; j++)
    {
        if (_individuals[dist[j]]->getCrowdingDistance() != INF)
        {
            _individuals[dist[j]]->setCrowdingDistance(_individuals[dist[j]]->getCrowdingDistance()/_nobj);
        }
    }
    return;
}

/* (Directly from Deb's code) Routine to compute crowding distance based on ojbective function values when the population in in the form of a list */
void Population::assignCrowdingDistanceList (void *plst, int front_size)
{
    int **obj_array;
    int *dist;
    int i, j;
    list *temp;
    list *lst;
    lst = (list*) plst;
    temp = lst;
    if (front_size==1)
    {
        _individuals[lst->index]->setCrowdingDistance(INF);
        return;
    }
    if (front_size==2)
    {
        _individuals[lst->index]->setCrowdingDistance(INF);
        _individuals[lst->child->index]->setCrowdingDistance(INF);
        return;
    }
    obj_array = (int **)malloc(_nobj*sizeof(int*));
    dist = (int *)malloc(front_size*sizeof(int));
    for (i=0; i<_nobj; i++)
    {
        obj_array[i] = (int *)malloc(front_size*sizeof(int));
    }
    for (j=0; j<front_size; j++)
    {
        dist[j] = temp->index;
        temp = temp->child;
    }
    assignCrowdingDistance(dist, obj_array, front_size);
    free (dist);
    for (i=0; i<_nobj; i++)
    {
        free (obj_array[i]);
    }
    free (obj_array);
    return;
}

void Population::assignRankAndCrowdingDistance()
{
    int flag;
    int i;
    int end;
    int front_size;
    int rank=1;
    list *orig;
    list *cur;
    list *temp1, *temp2;

    orig = (list *)malloc(sizeof(list));
    cur = (list *)malloc(sizeof(list));
    front_size = 0;
    orig->index = -1;
    orig->parent = NULL;
    orig->child = NULL;
    cur->index = -1;
    cur->parent = NULL;
    cur->child = NULL;
    temp1 = orig;
    for (i=0; i< size(); i++)
    {
        insert (temp1,i);
        temp1 = temp1->child;
    }
    do
    {
        if (orig->child->child == NULL)
        {
            _individuals[orig->child->index]->setRank(rank);
            _individuals[orig->child->index]->setCrowdingDistance(INF);
            break;
        }
        temp1 = orig->child;
        insert (cur, temp1->index);
        front_size = 1;
        temp2 = cur->child;
        temp1 = del (temp1);
        temp1 = temp1->child;
        do
        {
            temp2 = cur->child;
            do
            {
                end = 0;
                flag = checkDominance(*_individuals[temp1->index],*_individuals[temp2->index]);
                if (flag == 1)
                {
                    insert (orig, temp2->index);
                    temp2 = del (temp2);
                    front_size--;
                    temp2 = temp2->child;
                }
                if (flag == 0)
                {
                    temp2 = temp2->child;
                }
                if (flag == -1)
                {
                    end = 1;
                }
            }
            while (end!=1 && temp2!=NULL);
            if (flag == 0 || flag == 1)
            {
                insert (cur, temp1->index);
                front_size++;
                temp1 = del (temp1);
            }
            temp1 = temp1->child;
        }
        while (temp1 != NULL);
        temp2 = cur->child;
        do
        {
            _individuals[temp2->index]->setRank(rank);
            temp2 = temp2->child;
        }
        while (temp2 != NULL);
        assignCrowdingDistanceList(cur->child, front_size);
        temp2 = cur->child;
        do
        {
            temp2 = del (temp2);
            temp2 = temp2->child;
        }
        while (cur->child !=NULL);
        rank+=1;
    }
    while (orig->child!=NULL);
    free (orig);
    free (cur);
    return;
    
}

/* (Directly from Deb's code) Routine to compute crowding distance based on objective function values when the population in in the form of an array */
void Population::assignCrowdingDistanceIndices (int c1, int c2)
{
    int **obj_array;
    int *dist;
    int i, j;
    int front_size;
    front_size = c2-c1+1;
    if (front_size==1)
    {
        _individuals[c1]->setCrowdingDistance(INF);
        return;
    }
    if (front_size==2)
    {
        _individuals[c1]->setCrowdingDistance(INF);
        _individuals[c2]->setCrowdingDistance(INF);
        return;
    }
    obj_array = (int **)malloc(_nobj*sizeof(int*));
    dist = (int *)malloc(front_size*sizeof(int));
    for (i=0; i< _nobj; i++)
    {
        obj_array[i] = (int *)malloc(front_size*sizeof(int));
    }
    for (j=0; j<front_size; j++)
    {
        dist[j] = c1++;
    }
    assignCrowdingDistance(dist, obj_array, front_size);
    free (dist);
    for (i=0; i<_nobj; i++)
    {
        free (obj_array[i]);
    }
    free (obj_array);
    return;
}

/* (Directly from Deb's code) Routine to perform non-dominated sorting 
  We steal the unique pointers from parent.
*/
void Population::fillNondominatedSortFrom(Population &parent)
{
    int flag;
    int i, j;
    int end;
    int front_size;
    int archieve_size;
    int rank=1;
    list *pool;
    list *elite;
    list *temp1, *temp2;
    pool = (list *)malloc(sizeof(list));
    elite = (list *)malloc(sizeof(list));
    front_size = 0;
    archieve_size=0;
    pool->index = -1;
    pool->parent = NULL;
    pool->child = NULL;
    elite->index = -1;
    elite->parent = NULL;
    elite->child = NULL;
    temp1 = pool;
    for (i=0; i<parent.size(); i++) // Expect parent.size() == 2*popsz
    {
        insert (temp1,i);
        temp1 = temp1->child;
    }
    i=0;
    int upto_size = parent.size()/2;
    
    do
    {
        temp1 = pool->child;
        insert (elite, temp1->index);
        front_size = 1;
        temp2 = elite->child;
        temp1 = del (temp1);
        temp1 = temp1->child;
        do
        {
            temp2 = elite->child;
            if (temp1==NULL)
            {
                break;
            }

            do
            {
                end = 0;
                flag = checkDominance (*(parent._individuals[temp1->index]), *(parent._individuals[temp2->index]));
                if (flag == 1)
                {
                    insert (pool, temp2->index);
                    temp2 = del (temp2);
                    front_size--;
                    temp2 = temp2->child;
                }
                if (flag == 0)
                {
                    temp2 = temp2->child;
                }
                if (flag == -1)
                {
                    end = 1;
                }
            }
            while (end!=1 && temp2!=NULL);
            if (flag == 0 || flag == 1)
            {
                insert (elite, temp1->index);
                front_size++;
                temp1 = del (temp1);
            }
            temp1 = temp1->child;
        }
        while (temp1 != NULL);
        
        temp2 = elite->child;
        j=i;
        if ( (archieve_size+front_size) <= upto_size)
        {
            do
            {
                //copy_ind (&mixed_pop->ind[temp2->index], &new_pop->ind[i]);
                _individuals[i] = move(parent._individuals[temp2->index]);
                _individuals[i]->setRank(rank);
                archieve_size+=1;
                temp2 = temp2->child;
                i+=1;
            }
            while (temp2 != NULL);
            assignCrowdingDistanceIndices(j, i-1);
            rank+=1;
        }
        else
        {
            crowdingFillFrom(parent, i, front_size, elite);
            archieve_size = upto_size;
            for (j=i; j<upto_size; j++)
            {
                _individuals[j]->setRank(rank);
            }
        }
        temp2 = elite->child;

        do
        {
            temp2 = del (temp2);
            temp2 = temp2->child;
        }
        while (elite->child !=NULL);
    }
    while (archieve_size < upto_size);

    while (pool!=NULL)
    {
        temp1 = pool;
        pool = pool->child;
        free (temp1);
    }
    while (elite!=NULL)
    {
        temp1 = elite;
        elite = elite->child;
        free (temp1);
    }
    return;
}