#include "../include/population.h"
#include <exception>
#include <tgmath.h>
#include <Eigen/Dense>
using namespace Eigen;

// HARDCODED parameters
double OMEGA_HEIGHT = 10; // WEIGHT DIFFERENCIATION FOR CUT SELECTION (low omega = high selection)
float STORM_COEF_MORTALITY_BY_METER = 0.01; //0.03?

/* Generation of a new population
 *
 *
 */
population::population(species * i_species, unsigned int i_abs, unsigned int n_pops)
{
    //cout << "dat constructor" << endl;
    idsUpdate = false;
    t_species = i_species;
    abs = i_abs;
    cap = t_species->get_cap(); //TODO : proper logic for that
    growth = t_species->get_growth(); //TODO : proper logic for that

    for (int p = 0; p < n_pops; p++)
    {
         nb_father_by_pop.push_back(0);
         nb_mother_by_pop.push_back(0);
    }
}


population::~population()
{
    //Iterate on index and free all individuals
    for(const auto& i: index)
    {
        delete i.second;
    }
    for (auto i: individual_values)
    {
      delete i.second;
    }
}


/* Method to add an indvidual to a population
 * Adds the individual in the map with id as key
 * - Constant time if index has enought space
 * - Probably linear if reallocation occurs
 * - Overall amorted (reallocation typically doubles size)
 * - Erase if already exists (no reason to occur)
 */
void population::add_individual(individual* pInd)
{
    idsUpdate = true;
    if (pInd == NULL) fail("add_individual: pInd is NULL");
    index[pInd->get_id()] = pInd;
    return;
}

void population::add_individual_buffer(individual* pInd)
{
    if (pInd == NULL) fail("add_individual: pInd is NULL");
    indexNext[pInd->get_id()] = pInd;
    return;
}


/* Method to remove an individual from a population using its unique id
 * Does not destroys the individual, merely removes it from index
 * Warning, if pointer returned is lost, there is no way to free the individual
 */
individual * population::remove_individual(indId id)
{
    //cout << "remove really" << id << "(" << get_nInd() << ")" << endl;
    idsUpdate = true;
    // Get pointer
    individual * pInd = get_pInd(id);
    // Delete from umap
    index.erase(id);
    return pInd;
}

/* Removes individual from index and destroys it
 *
 */
void population::destroy_individual(indId id)
{
    //cout << "effective delete " << id << endl;

    delete remove_individual(id);
}


/* Removes individual from index::iterator
 * Returns next iterator
 */
indMap::iterator population::destroy_from_it(indMap::iterator it)
{
    idsUpdate = true;
    // Get pointer & deletes object
    delete it->second;
    return index.erase(it);
}


/* Returns a ref to a vector containing all indId in the order of the container
 * Is updated only when needed
 */
const vector<indId>& population::get_ids()
{
    // If index has been modified, recreate the vector
    //cout << "IUg:" << idsUpdate << endl;
    try{
        if (idsUpdate)
        {
            //cout << "IDSUPDATE" << endl;
            ids.clear();
            ids.reserve(get_nInd());
            for(auto i: index) ids.push_back(i.first);
            idsUpdate = false;
        }
    }
    catch (exception& e)
    {
        cout << "Standard exception: " << e.what() << endl;
        ids.clear();
        ids.reserve(0);
        idsUpdate = false;
    }
    return ids;
}



/* New Demography function
 * Uses cap and growth to calculate a delta of pop for the next generation
 * Uses flux matrix to correct individuals contributing to growth and to cap
 * Update the pop internal delta and returns the value
 */
int population::demography_new()
{
    targetPop = t_species->getAgeSizes()[0];
    return targetPop;
}

void population::force_targetPop(int tPop)
{
    if (tPop >= 0)
    {
        targetPop = tPop;
    }
    else
    {
        cout << "Tpop not authorized:" << tPop << endl;
    }
}


/* Adjusts population so that number of individuals matches
 * the target defined by demography.
 * Spawns all new individuals.
 */
 // WORKS ONLY FOR GENERATIONNAL MODEL
void population::adjust(landscape &myMap, mClock &m_clock)
{
    int newInd = 0;
    // reset nb_father_by_pop
    for (int i=0; i< nb_father_by_pop.size(); i++)
    {
        nb_father_by_pop[i] = 0;
        nb_mother_by_pop[i] = 0;
    }

    while(targetPop > newInd)
    {
        add_individual_buffer(spawn(myMap, m_clock));
        newInd++;
    }

    return;// newPop;//useless?
}

void population::set_new_population()
{
    //cout << "OLD POP SIZE:" << get_nInd() << endl;
    index = indexNext;
    indexNext.clear();
    //cout << "NEW POP SIZE:" << get_nInd() << endl;
    idsUpdate = true;
}

void population::adjust_generationmode(landscape &myMap, mClock &m_clock)//should be renamed "cut"
{
    //cout << "ADJUST GM" << endl;
    double nInd = get_nInd();

    individual_index * heightIndex = get_index_safe("height");

    int nToRemove = nInd - targetPop - killedBuffer.size();

    int cutmode = 1;
    bool verbose = false;

    if (cutmode == 0)
    {
        // STRICT MAX SELECTION ON HEIGHT
        for (int i=0; i < nToRemove; i++)
        {
            killedBuffer.insert(heightIndex->get_pair(i).second);
            //cout << "Will kick ind" << heightIndex->get_pair(i).second << " because shorter" << endl;
        }
    }
    else
    {
        // RANDOMIZED MAX SELECTION ON HEIGHT
        vector<float> heightRatio;
        float heightMax = heightIndex->get_max();
        float sumRatio = 0;
        vector<int> possibleInds;
        for (int i=0;i<nInd;i++)
        {
          //float indScore = 1.01-heightIndex->get_pair(i).first/heightMax; // SCORE FUNCTION MAY CHANGE
          float indScore = 1.001-exp(-pow((heightIndex->get_pair(i).first-heightMax)/OMEGA_HEIGHT, 2)/2);
          //cout << heightIndex->get_pair(i).first << " vs " << heightMax << " : " << indScore << endl;

          possibleInds.push_back(i);
          heightRatio.push_back(indScore);
          //cout << "i " << i << " " << indScore << endl;
          sumRatio += indScore;
        }
        if (verbose) cout << "sumRatio " << sumRatio << endl;
        for (int i=0;i<nToRemove;i++)
        {
          if (verbose) cout << "NTR" << i << " ";
          float selectedSum = Random()*sumRatio; // Pick a random value between 0 and the ratio sum
          if (verbose) cout << "selsum " << selectedSum << " ";
          float currentSum = 0;
          float currentPos = -1;
          int currentInd = -1;
          while (selectedSum > currentSum)
          {
            if (currentPos >= possibleInds.size())
            {
              cout << "PROBLEM WITH IND PICKING" << endl;
              exit(0);
            }
            else if (currentPos == possibleInds.size()-1)
            {
                  cout << "SHADY ZONE, BUGGED CAUSE OF BORDER EFFECT. Force loop exit" << endl;
                  cout << "CI: " << currentInd << "SU/SR:" << selectedSum << "/" << sumRatio << endl;
                  currentSum = selectedSum;
            }
            else
            {
                 currentPos++;
                 currentInd = possibleInds[currentPos];
                 currentSum += heightRatio[currentInd];
                 //cout << "cp " << currentPos << " CI " << currentInd << " CS " << currentSum << endl;
            }
          }
          //currentInd = possibleInds[currentPos];
          //found the wanted position
          killedBuffer.insert(heightIndex->get_pair(currentInd).second);
          sumRatio -= heightRatio[currentInd];
          if (verbose) cout << "sumRatio refreshed " << sumRatio << endl;
           //heightRatio.erase(heightRatio.begin() + currentPos);
          if (currentPos >= possibleInds.size())
          {
                  cout << "CP " << currentPos << "not in range of possibleInds: " << possibleInds.size() << endl;
                  exit(0);
          }
          possibleInds.erase(possibleInds.begin() + currentPos);
          if (verbose) cout << "delpos " << currentPos << endl;
       }
    }

    /*while(nInd - targetPop > killedBuffer.size())
    {
       //cout << "killrandom" << endl;
       //kill_random(); // TODO DO FOR ADULT/OLD NOT KILL RANDOMLY BUT BY PHENO VAL
       indId randId = pick_random();
       killedBuffer.insert(randId);
       //nInd--;
    }*/

    idsUpdate = true;
}


/* Storm emulation: Randomly cuts high trees
*/
int population::storm_impact()
{
  double nInd = get_nInd();

  individual_index * heightIndex = get_index_safe("height");
  // linear relationship : mort = 0.02*h // TODO better formula ?
  double nKilled = 0;
  for (int i=0;i<nInd;i++)
  {
    float currentH = heightIndex->get_pair(i).first;
    if (currentH*STORM_COEF_MORTALITY_BY_METER > Random()) // TEST IF TREE KILLED
    {
      killedBuffer.insert(heightIndex->get_pair(i).second);
      nKilled++;
    }
  }
  cout << nKilled << " in pop " << abs << " because of storm" << endl;

  idsUpdate = true;
  return nKilled;
}


/* Selects a random individual
*/
indId population::pick_random()
{
    if(index.size() <= 0) fail("pick_random: Population is empty !");
    // Pick random value
    int roll = floor(Random() * index.size());
    // Get iterator, dereference, take first field
    indId pick = next(begin(index), roll)->first;
    //cout << "PICK " << pick << endl;
    return pick;
}



/* Temp function that deletes a random individual
*/
void population::kill_random()
{
    //cout << "KILL RANDOM" << endl;
    //destroy_individual(pick_random());
    killedBuffer.insert(pick_random());

    //idsUpdate = true; //externalize ?
}



/* Select a pop based on number of individuals * flux matrix
 * Works for both MALE and FEMALE (chnges the flux matrix accordingly)
 * Cost: On copy of matrix (size npop) in fluxMat::draw
 * TODO: Remove passing of landscape by returns popId and not population*
 */


 vector<double> population::weight_without_mother(vector<double> originWeight, int motherPop)
 {
   if (originWeight[motherPop] <= 0)
   {
      cout << "Bug: weight for " << motherPop << "(" << originWeight[motherPop] << ") was already null or less." << endl;
      for (int i=0;i<originWeight.size();i++)
      {
         cout << originWeight[i];
      }
   }
   else
   {
     originWeight[motherPop]--;
     /*if (originWeight[abs] <= 0)
     {
         cout << abs << "muted since only one element in mother pop. ";
     }*/
   } // NOT OK ALONE : CAN STILL EVENTUALLY PICK BAD PARTNER
   return originWeight;
 }


population * population::select_pop_demo(landscape &myMap, int sex, int motherPop)
{
    // 1 - Ref to the demography of all pops of this species
    vector<double> weightsRef = t_species->get_demo(); // was vector<double>&

    if (sex==MALE)
    {
      weightsRef = this->weight_without_mother(weightsRef, motherPop);
    }
    /*int j = 0;
    for (std::vector<double>::const_iterator i = weightsRef.begin(); j < 15; ++i)
    {
	        std::cout << *i << ' ';
		j++;
    }
    std::cout << endl;*/

    // 2 - Select mother pop index using flux matrix & demo weighting, 0 is female sex
    //cout << "WREF" << weightsRef.size() << endl;

    int idPop = t_species->select_weighted_pop(get_abs_pos(), sex, weightsRef);
    //if ((idPop != abs) && (sex==1)) cout << "IDP:" << idPop << "vs" << abs << endl;
    if (idPop == -1) fail("select_pop_demo: No pop found !");
    // 3 - Get pop from index
    cell * pCell = myMap.get_cell(idPop);
    population * pPop = pCell->get_pop(t_species);
    // 4 - Tests
    if(pPop == NULL) fail("select_pop_demo: Invalid pop");
    //if(pPop->get_nInd() == 0) fail("WARNING empty pop !"); //This should NEVER happen

    return pPop;
}


/* Selects a pop using assortMating (demogaphy is only calculated for a range of values)
 * If we need pop of father, "this" is the population of mother
 * assortValue = reference value, = pheno value of trait for mother
 * Tolerance value can be modified in the calling function !
 */
population * population::select_pop_assortMating(double assortValue, landscape &myMap,
                                             int sex, double& tolerance, int motherPop)
{
    // Get global index of values for the trait values
    string assortTraitId = t_species->get_assortMating() + t_species->get_assortSuffix(); //TODO: +"pheno" is quite ugly

    group_index * gi = t_species->get_index(assortTraitId);
    // Extract number of indivs close to the mother value
    // (cost: new vector of nCells: 2 binaries search per population)
    double min = assortValue - tolerance;
    double max = assortValue + tolerance;
    vector<double> weights = gi->count_ind_range(min, max);

    //cout << "== SELECTPOPASSORT cut " << min << " to " << max << ";";
    weights = this->weight_without_mother(weights, motherPop);

    /*cout << "assort " << assortValue << " tol " << tolerance << " min " << min << " max " << max << endl;
    cout << "WEIGHTS:";
    for (int i=0;i<weights.size();i++)
    {
        cout << weights[i] << ", ";
    }
    cout << endl;*/

    // Select pop from weights & flux matrix
    int idPop = t_species->select_weighted_pop(get_abs_pos(), sex, weights);
    // If a population is found, return pointer

    if(idPop >= 0)
    {
        return myMap.get_cell(idPop)->get_pop(t_species);
    }
    else
    {
      if (t_species->is_assort_strategy("end"))
      {
          cout << "[BUG] No pop found with given homogamy strength.\nEnd of program." << endl;
          exit(0);
      }
    }
    // If nobody in the range: increase tolerance


    // GIVE MORE INFOS ABOUT DENIAL
    cout << "[WARNING] In pop " << motherPop << ": no potential mate for interval " << min << " to " << max << ".";
    if (t_species->is_assort_strategy("unmatchable"))
    {
        cout << " Female excluded from pool for this step." << endl;
    }

    if (t_species->is_assort_strategy("expand_tolerance"))
    {
        // If nobody in the range: increase tolerance
        min = gi->get_rangeMin();
        max = gi->get_rangeMax();
        tolerance += (max - min)/10; // TODO: do that properly
        cout << "Reevaluating tolerance: " << tolerance << endl;
        return  select_pop_assortMating(assortValue, myMap, sex, tolerance, motherPop);
    }
    // Relaunch function with the new tolerane
    else
    {
        return nullptr;
    }
}


/* Generates a new individual in the pop from father and mother pointers
 * The individuals is added to the current population (this)
 */
individual* population::generate_ind(individual& motherRef, individual& fatherRef,
                              mClock &m_clock)
{
    individual * pInd = new individual(motherRef, fatherRef, m_clock, t_species);
    return pInd;
}





/* Selects an individuals inside the pop in a tolerance range for assortMating
 * The final ponderation between individuals in the range is based on fitness
 */
indId population::pick_assortMating(double assortValue, double tolerance, bool needMotherCut, indId refMother)
{
    double min = assortValue - tolerance;
    double max = assortValue + tolerance;
    string keyRange = t_species->get_assortMating() + t_species->get_assortSuffix();
    string keyWeight = "cumul_fitness";

    //Get index from the current pop
    individual_index * iiRange = get_index(keyRange);
    individual_index * iiWeight = get_index(keyWeight);
    if(iiRange == NULL or iiWeight == NULL)
        fail("population::spawn_assortMating: Index not found");

    return pick_weighted_range(*iiRange, *iiWeight, min, max, needMotherCut, refMother);
}





/* Selects individual in a range of values from index iiRange
 * Then randomly picks one, weighted by values of index iiWeight
 */
indId population::pick_weighted_range(individual_index &iiRange,
                                            individual_index &iiWeight,
                                            double start, double end, bool needMotherCut, indId refMother)
{
    // Gets list of individuals id in the range
    vector<indId> vIndId;
    bool success = iiRange.get_ind_range(start, end, vIndId);
    if(!success) fail("WARNING, no individual selected by pick_weighted_range");
    /* TODO : check how to handle cases with no available matchs*/

    // Get values of selected individuals (cumulative sum on the way)
    vector<double> values;
    if (needMotherCut)
    {
      values.reserve(vIndId.size()-1);
    }
    else
    {
      values.reserve(vIndId.size());
    }
    double sum = 0.0;
    vector<indId> vIndIdClean;

    for(indId ind: vIndId)
    {
      if (ind != refMother || !needMotherCut)
      {
        //cout << "ind:" << ind << ", refMother:" << refMother << ", needMotherCut;" << needMotherCut << endl;
        values.push_back(sum += iiWeight.get_val(ind));
        vIndIdClean.push_back(ind);
      }
    }
    // Then divide by max
    for(auto& val: values)
    {
        val /= sum;
    }



    // Draw an individual (Should not == end())
    auto it = upper_bound(values.begin(), values.end(), Random());

    if(it == values.end()) fail("pick_weighted_range: Out of range.");

    return vIndIdClean[it - values.begin()];
}


/* Rand pick an individual from current population based on fitness
 * Complexity: log(nind)
 */
indId population::pick_fitness(bool needMotherCut, indId refMother)
{
    // We used pre-calculated cumulative sum of fitness
    if (needMotherCut)
    {
        individual_index * pii = get_index_safe("fitness");
        //indId picked;
        vector<double> vals = pii->get_values();
        /*cout << "VALS ";
        for (int i=0;i<vals.size();i++)
        {
            cout << vals[i] << " ";
        }
        cout << endl;*/
        vector<double> values;
        double sum = 0.0;

        // Get the iterator and check it is not equal to end()
        vector<indId> vIndIdClean;

        for(indId ind: pii->get_inds())
        {
            if (ind != refMother)
            {
              //cout << "ind:" << ind << ", refMother:" << refMother << ", needMotherCut;" << needMotherCut << endl;
              values.push_back(sum += pii->get_val(ind));
              vIndIdClean.push_back(ind);
            }
        }
        // Then divide by max
        for(auto& val: values) val /= sum;

        if (sum==0) cout << "[BUG] SUM NULL IN PICK FITNESS, can't divide" << endl;

        auto iter = upper_bound(values.begin(), values.end(), Random());
        if(iter ==  values.end())
        {
          cout << "pick_fitness: No ind found (" << to_string(abs) << ")" << endl;
          return 0;
        }
        return vIndIdClean[iter-values.begin()];
    }
    else
    {
        individual_index * pii = get_index_safe("cumul_fitness");
        indref::iterator iter = pii->get_upper_bound(Random());
        if(iter == pii->get_end()) fail("pick_fitness: No ind found");
        return iter->second;
    }
}


/* Find a parent based on fitness
 * TODO: if select_pop_demo returned popID we would have not need to pass myMap
 */
pair<population*, indId> population::select_parent(int sex, landscape &myMap, int popMother, indId numMother)
{
    // Select population of origin, based on number of individuals * flux_matrix
    population * pPop = select_pop_demo(myMap, sex, popMother);
    //if (pPop->get_abs_pos() != abs) cout << "pPop" << pPop->get_abs_pos() << " VS " << abs << endl;
    if(pPop->get_nInd() == 0) fail("spawn: Empty population"); //TODO: not nInd but indivdual_index.size() => does not change
    // Select parent inside the selected pop, based of fitness
    bool needMotherCut = false;
    if (popMother == pPop->get_abs_pos() && sex == MALE)
    {
      needMotherCut = true;
    }
    indId id = pPop->pick_fitness(needMotherCut, numMother);

    if (t_species->is_assort_strategy("unmatchable"))
    {
        set<indId> unmatchable = t_species->get_unmatchable();
        if (unmatchable.find(id) == unmatchable.end())
        {
           return pair<population*, indId>(pPop, id);
        }
        else
        {
           return select_parent(sex, myMap, popMother, numMother);
        }
    }
    else
    {
        return pair<population*, indId>(pPop, id);
    }
}

pair<population*, indId> population::select_father_assortMating(pair<population*, indId> mother, string assort, landscape &myMap)
{
    pair<population*, indId> father;
    // Get assortMating value of mother (mother is in popMother !)
    //double val = mother.first->get_index_safe(assort+"-pheno")->get_val(mother.second);
    string suffixAssort = t_species->get_assortSuffix();
    double val = mother.first->get_index_safe(assort+suffixAssort)->get_val(mother.second);
    // tolerance might be changed in select_pop_assortMating if multiples iterations
    double tolerance = t_species->get_assortThres();
    // Select pop father: (tolerance is passed by ref, might be changed)
    father.first = select_pop_assortMating(val, myMap, MALE, tolerance, mother.first->get_abs_pos());
    if (!father.first && t_species->is_assort_strategy("unmatchable"))
    {
       //cout << "[WARNING] NO FATHER FOR GIVEN FEMALE, select another female" << endl;
       father.second = 0;
        return father;
    }
    if(father.first->get_nInd() == 0) fail("spawn: Empty father population"); //TODO: not nInd but indivdual_index.size() => does not change
    // Get father. Here tolerance might have been augmented
    bool needMotherCut = false;
    if (father.first->get_abs_pos() == mother.first->get_abs_pos())
    {
      needMotherCut = true;
    }
    father.second = father.first->pick_assortMating(val, tolerance, needMotherCut, mother.second);

    return father;
}


/* Spawns a new individual in this population
 * TODO: Make a function: "select_parent_assortMating?
 */
individual* population::spawn(landscape &myMap, mClock &m_clock)
{
    // Select mother
    pair<population*, indId> mother = select_parent(FEMALE, myMap, 0, 0);
    //cout << "MS" << mother.second << " (" << mother.first->get_abs_pos() << ") in pop " << abs << endl;
    if (mother.first->is_ind(mother.second))//mother.second != 0 ||
    {
        individual& rMother = mother.first->get_rInd(mother.second);
        // Check for selfing
        if(Random() < t_species->get_selfing())
        {
            return generate_ind(rMother, rMother, m_clock);
            //return true;
        }
        // Select father
        pair<population*, indId> father;
        string assort = t_species->get_assortMating();
        // No assortMating
        if(assort == "") father = select_parent(MALE, myMap, mother.first->get_abs_pos(), mother.second);
        else // assortative mating
        {
            father = select_father_assortMating(mother, assort, myMap);
            if (!father.first && t_species->is_assort_strategy("unmatchable"))  // was unable to find a father to the given mother
            {
                 t_species->add_unmatchable(mother.second);
                 //cout << "New spawn..." << endl;
                 return spawn(myMap, m_clock);
            }
        }


        if (father.first->is_ind(father.second)) // father.second != 0 ||
        {
          //report father origin information
            int npopfa = father.first->get_abs_pos();
            int npopmo = mother.first->get_abs_pos();

            if (father.second == mother.second)
            {
              cout << "[BUG] MOTHER = FATHER OUTSIDE OF SELFING (" << father.second  << " from pop " << npopfa << " VS. " << mother.second  << " from pop " << npopmo << ")" << endl;
            }
            /*else
            {
              cout << "  [NORMAL] MOTHER != FATHER OUTSIDE OF SELFING (" << father.second  << " from pop " << npopfa << " VS. " << mother.second  << " from pop " << npopmo << ")" << endl;
            }*/

            nb_father_by_pop[npopfa]++;
            nb_mother_by_pop[npopmo]++;

            /*if (father.first->get_abs_pos() != abs)
            {
                cout << "Journal of pop " << abs << " : External father from pop " << father.first->get_abs_pos() << endl;
            }
            if (mother.first->get_abs_pos() != abs)
            {
                cout << "Journal of pop " << abs << " : External mother from pop " << mother.first->get_abs_pos() << endl;
            }*/

            //cout << "NPOP" << npop << "-" << nb_father_by_pop[npop] << endl;
            //cout << "FS" << father.second << endl;
            individual& rFather = father.first->get_rInd(father.second);
            return generate_ind(rMother, rFather, m_clock);
            //return true;
        }
        else
        {
            cout << "Papaoutai" << endl;
            exit(0);
            //return false;
        }
    }
    else
    {
        cout << "No mother found" << endl;
        exit(0);
    }
}
/******************* End reproduction *****************************************/


int population::abs_select(string origin, float threshold, float ratio)
{
    //add to destroy list or effective delete?
    //if (origin == "insect")
    return kill_range(origin+"-pheno", -100000, threshold, ratio);
}

/* Calculates age based on birth date
 * Stores values in a sorted individual index
 */
void population::make_ages_index(mClock &m_clock)
{
    vector<double> ages;
    ages.reserve(get_nInd());

    age = -1;
    for(const auto& i: index)
    {
        int currAge = (i.second)->get_age();
        if (age == -1)
        {
            age = currAge; // REFRESH POP AGE
        }
        ages.push_back(currAge);
    }
    add_index("age", ages, get_ids(), true);
    return;
}

void population::update_ages(mClock &m_clock)
{
    for(const auto& i: index)
    {
        (i.second)->age_update(m_clock, t_species);
    }
    return;
}


/* Deletes all individuals in the range [mini, maxi]
 * TODO : check exact boundaries [[, [] or ]] ?
 */

int population::purge_killBuffer()
{
    int kbSize = killedBuffer.size();
    if (kbSize > 0)
    {
        cout << "effective kill trees in pop "<< abs << ", buffer of size " << kbSize << endl;
        //for (int i=0;i<killedBuffer.size();i++)
        for (auto i : killedBuffer)
        {
            //cout << "destroying " << i << endl;
            destroy_individual(i);
            idsUpdate = true;
        }
        killedBuffer.clear();
        //cout << "clear buffer, new size " << killedBuffer.size() << endl;
    }
    return kbSize;
}

int population::kill_range(string key, double mini, double maxi)
{
  //RETURNS the number of killed elements
    //cout << "Kill range " << mini << " to " << maxi << endl;
    individual_index * pIndex = get_index_safe(key);
    indref::iterator iStart = pIndex->get_lower_bound(mini);
    indref::iterator iEnd = pIndex->get_upper_bound(maxi);

    int killed = 0;
    for(indref::iterator it = iStart; it != iEnd; it++)
    {
        indId i = it->second;
        //cout << "kill ind " << i << " for reason " << key << " value between " << mini << " and " << maxi << endl;
        killedBuffer.insert(i);
        killed++;
    }
    if (killed > 0)
    {
        // TODO INJECT TXT IN OUTPUT FILE

        cout << "will kill " << killed << " inds in pop " << abs << "for reason " << key << " value between " << mini << " and " << maxi << endl;
    }
    return killed;
}

int population::kill_range(string key, double mini, double maxi, float ratio)
{
    //cout << "Kill range " << mini << " to " << maxi << endl;
    individual_index * pIndex = get_index_safe(key);
    indref::iterator iStart = pIndex->get_lower_bound(mini);
    indref::iterator iEnd = pIndex->get_upper_bound(maxi);

    int killed = 0;
    for(indref::iterator it = iStart; it != iEnd; it++)
    {
        if (Random() < ratio)
        {
            indId i = it->second;
            //cout << "kill ind " << i << " in pop " << abs << " for reason " << key << " value between " << mini << " and " << maxi << endl;
            killedBuffer.insert(i);
            killed++;
        }

    }
    if (killed > 0)
    {
        cout << "will kill " << killed << " inds in pop " << abs << " for reason " << key << " value between " << mini << " and " << maxi << endl;
    }
    //cout << "END OF KILL RANGE RATIO " << ratio << endl;
    return killed;
}

/* Calculate additive value of a trait for each individual in the pop
 * Takes an empty vector as entry (or will empty it)
 */
void population::calc_additive_values(vector<double>& addVals, trait* pTrait)
{
    addVals.clear();
    //cout << "NIND" << get_nInd() << endl;
    addVals.reserve(get_nInd());

    //For each individual (i = pair<indId, individual*>)
    for(auto i: index)
    {
        //cout << "Isec ID" << i.second->get_id() << " GS " << i.second->gen_size() << " A " << i.second->get_age() << endl ;
        addVals.push_back(pTrait->calc_add_value(*(i.second), pTrait->get_E(abs)));
        /* DEBUG */ if (std::isnan(addVals.back()))
        /* DEBUG */      fail("calc_additive_values: nan");
    }
    return;
}





/* Calculates additives values for multiple traits
 * Indexing isdone here if needed calc_additive_values because it saves
 * making a vector of indId each time (we pass the same by ref)
 */
void population::calc_add_multiple(vector<vector<double>>& values, vector<trait *>& traits)
{
    values = vector<vector<double>>(traits.size());
    //cout << "DEBUG TS " << traits.size() << endl;
    // For each trait
    for(unsigned int i = 0; i < traits.size(); ++i)
    {
        // Calculate additive values of all individuals
        //cout << "DEBUG pre calc add" << endl;
        calc_additive_values(values[i], traits[i]);
        //cout << "DEBUG post calc add" << endl;
        // Indexing, optional
        // TODO : specifiy at which step we index
        if(traits[i]->is_indexed())
        {
            string name = traits[i]->get_name() + "-add";
            //sorted, no reversed index
            // TODO: care about rev index or not
            add_index(name, values[i], get_ids(), true);
        }
    }
    return;
}





/* Placeholder for epistasy and dominance implementation
 */
void population::calc_genetic_values(vector<double>& add_values, trait * pTrait)
{
/*
    double Dom = 0.0;
    double Epi = 0.0;
    for(double& val : add_values)
    {
        val += Dom + Epi;
    }
    return;
*/
    return;
}



/* Placeholder for epistasy and dominance implementation
*/
void population::calc_gen_multiple(vector<vector<double>>& values, vector<trait *>& traits)
{

    return;
}



void population::calc_phenotypic_values_epsi(vector<double>& genetic_values, vector<double>& epsi_values, double E = 0.0, double Zm = 0.0)
{
    //for(double& val: genetic_values)
    for(unsigned int i = 0; i < genetic_values.size(); ++i)
    {
        genetic_values[i] = genetic_values[i] + E + Zm + epsi_values[i];
    }

    return;
}


/* Calculates phenotypic value, E is optional
 * Everything is done in place
 */
void population::calc_pheno_multiple(vector<vector<double>>& values, vector<trait *>& traits, vector<vector<double>>& epsi)
{
    if(values.size() != traits.size()) fail("calc_add multiple : Error");
    //vector<vector<double>> valuesForassort(get_nInd());
    vector<vector<double>> valuesForassort(traits.size());
    //Calculate additive values of a subset of traits for all individuals
    double E, Zm, vEps;
    vector<vector<double>> dz(traits.size());
    for(unsigned int i = 0; i < traits.size(); ++i)
    {
        //Non heritable phenotipic plasticity :
        if(traits[i]->is_nhpp())
        {
            E = traits[i]->get_E(abs);
        }
        else
        {
            E = 0.0;
        }

        Zm = traits[i]->get_meanZ();
        vEps = traits[i]->get_varEps();

        for(unsigned int vi = 0; vi < values[i].size(); ++vi)
        {
            float eps = sqrt(vEps)*gasdev();
            epsi[i].push_back(eps);
        }

        //Calc pheno value
        if(t_species->get_assortMating() != "")
        {
            valuesForassort[i] = values[i];
            calc_phenotypic_values_epsi(valuesForassort[i], epsi[i], traits[i]->get_E(abs), Zm);
        }

        calc_phenotypic_values_epsi(values[i], epsi[i], E, Zm);

        // QUICK Computation for z-zopt
        double theta = traits[i]->get_Zopt(abs);
        //cout << "T" << endl;
        for(unsigned int v = 0; v < values[i].size(); ++v)
        {
          double dzv = values[i][v] - theta;
          dz[i].push_back(dzv);
        }

        //Indexing
        // TODO: frequency, rev index
        if(traits[i]->is_indexed())
        {
            string name = traits[i]->get_name() + "-pheno";
            add_index(name, values[i], get_ids(), true); //sorted

            if(t_species->get_assortMating() != "")
            {
                string name = traits[i]->get_name() + "-phenoWithE";
                add_index(name, valuesForassort[i], get_ids(), true); //sorted
            }

            name = traits[i]->get_name() + "-epsi";
            add_index(name, epsi[i], get_ids(), false); //not sorted

            name = traits[i]->get_name() + "-dz";
            add_index(name, dz[i], get_ids(), false); //not sorted
        }
        //cout << "endT" << endl;
    }
    return;
}

float heightGrowth(int age, float H, float rate, int x0)
{
    /*cout << "age" << age << " ; ";
    cout << "H" << H << " ; ";
    cout << "rate" << rate << " ; ";
    cout << "x0" << x0 << " ; ";*/
    float delta =H/(1+exp(-rate*(age-x0)));
    //cout << "delta " << delta << " || ";
	return delta;
}

/*float getTheoricalHeight(int age, indId i, R, rate)
{
}*/


void population::calc_height()
{
    vector<double> height;
    height.reserve(get_nInd());

    bool isT = true;

    individual_index * hmaxIndex;
    individual_index * rateIndex;
    if (isT)
    {
      rateIndex = get_index_safe("T4-pheno");
      hmaxIndex = get_index_safe("T3-pheno");
    }
    //individual_index * hlIndex = get_index_safe("T5-pheno");
    int x0 = 0; // TODO automatic ageMax/2?

    bool detectedDiscrep = false;

    for(const auto& i: index)
    {
        //int age = ; already in pop?

        float H;
        float rate;
        if (isT)
        {
          rate = rateIndex->get_val(i.first);
          H = hmaxIndex->get_val(i.first);
        }
        else
        {
          rate = 0.02;
	        H = 25;
        }

        if (rate < 0)
        {
          //rate = 0;
          detectedDiscrep = true;
        }
        //float rate = max(0, rateIndex->get_val(i.first));
        //float x0 = hlIndex->get_val(i.first);

        float thHT = heightGrowth(age, H, rate, x0);
        float thHTb = heightGrowth(age-1, H, rate, x0);
        float coefGrowth = thHT - thHTb;

        float oldH = (i.second)->get_height();
        // if generated with age positive, emulate old height following theoretical curve
        if (age > 1 && oldH == 0)
        {
            oldH = thHT;
        }
        float handicap = (i.second)->get_handicap();
	//cout << "handicap " << handicap << " for ind " << i.first << endl;
        float newH = oldH + handicap * coefGrowth;
        if (newH < oldH && !detectedDiscrep)
        {
          cout << "[ERROR] height can not decrease: oldH:" << oldH << " vs newH:" << newH << endl;
          detectedDiscrep = true;
        }
        (i.second)->set_height(newH);
        height.push_back(newH);
    }
    /*if (detectedDiscrep)
    {
        cout << "[ERROR] Negative growth rate" << endl;
    }*/

    add_index("height", height, get_ids(), true);
    return;
}

void population::refresh_handicap()
{
    for(const auto& i: index)
    {
        (i.second)->set_handicap(1);
    }
    return;
}

void population::impact_height(string trait, float cutValue, float handicapImpact)
{
    individual_index * phenoIndex = get_index_safe(trait+"-pheno");

    //cout << "CHECK PHENO:";
    for (const auto& i: index)
    {
        double handicapImpactLocal = (i.second)->get_handicap();
        //cout << i.first << "->" << phenoIndex->get_val(i.first) << " || ";
        if (phenoIndex->get_val(i.first) < cutValue)
        {
            handicapImpactLocal *= handicapImpact;
        }
        (i.second)->set_handicap(handicapImpactLocal);
    }
    //cout << endl;
    return;
}


//vectorial version of the fitness computation
//need modification of fitValues ?
void population::calc_fitness_multi_vector(vector<double>& fitValues,
                                    const vector<vector<double>>& traits_values,
                                    const vector<trait *>& traits,
                                    const vector<double>& omatrix)
{
    // Needs at least one trait
    if(traits_values.size() == 0) fail("Error, no trait found.");

    // New vector for results
    fitValues.clear();
    fitValues.reserve(get_nInd()); //Reserve for nInd


    const unsigned int nt = traits.size();
    MatrixXf matOm(nt, nt); //matrix format = y,x ?

    for(unsigned int t = 0; t < nt; ++t) {
        for(unsigned int u = 0; u < nt; ++u) {
            matOm(t, u) = omatrix[u*nt + t];
        }
    }


    VectorXf z(nt);
    VectorXf theta(nt);
    VectorXf dtheta(nt);
    for(unsigned int t = 0; t < nt; ++t)
    {
        theta[t] = traits[t]->get_Zopt(abs);
        //cout << "DBG:theta t at " << abs << " is " << theta[t] << endl;
    }
    // Iterate on individuals
    float wz;
    float meanWz = 0;
    int sizePop = traits_values[0].size();
    for(unsigned int i = 0; i < sizePop; ++i)
    {
        for(unsigned int t = 0; t < nt; ++t)
        {
            z[t] = traits_values[t][i];
            //if ((abs==9) && (i==0)) cout << " VF " << var_pheno[t] << " > " << sqrt(var_pheno[t]) << " ; ";
            if (var_pheno[t] == 0)
            {
                if (sizePop == 1)
		{
			cout << "[WARNING] pop of size 1, variance of pop artificially set to 1 for fitness computation" << endl;
			var_pheno[t] = 1;
		}
		else
		{
	                cout << "VARIANCE IS NULL" << endl;
	                exit(0);
		}
            }
            dtheta[t] = (z[t] - theta[t])/sqrt(var_pheno[t]); // (bug when var instead of sqrt(var), val too little?
        }
        wz = exp(-0.5*(dtheta.transpose())*matOm.inverse()*dtheta);
        meanWz += wz;
        fitValues.push_back(wz);
    }
    return;
}


/* General function to calc all traits values of a given specie;
 * The list might change given the age class
 */
void population::calc_traits()
{
    // 0 - Get age to know if trait computation is relevant
    int minTraitAge = 0;//t_species->getAgeEnds()[0]-1;
    //cout << "calc_trait: age " << age << "vs" << minTraitAge << endl;

    // 1 - Get traits to calculate fitness
    // TODO: We might not want to calculate them all depending on the age
    vector<trait*> selTraits;
    t_species->get_selTraits(selTraits);

    if (age >= minTraitAge)
    {
        // 2 - Calculate additive values of a subset of traits for all individuals
        // Include indexing if in the trait options
        vector<vector<double>> values(selTraits.size());
        calc_add_multiple(values, selTraits);


        // 3 - Calculate genetic value
        //calc_genetic_values(values); //DO NOT USE, no dominance or epis now
        //Save geneti value
        //save genetic
        // 3 (beta) - Calculate the random part
        vector<vector<double>> epsi_values(selTraits.size());

        // 4- Calc phenotypic value
        calc_pheno_multiple(values, selTraits, epsi_values);
        //4-1 Compute global values related to phenotypic value for variance computation
        vector<double> mean_pheno(selTraits.size());
        int nInd = index.size();
        for (int t=0;t<selTraits.size();t++)
        {
            double tmpSum = 0;
            for(int i=0 ; i<nInd ; i++)
            {
                tmpSum += values[t][i];
            }
            mean_pheno[t] = tmpSum / nInd;
        }

        var_pheno.reserve(selTraits.size());
        //cout << "VAR COMP ";
        for (int t=0;t<selTraits.size();t++)
        {
            double tmpVar = 0;
            float d;
            for(int i=0 ; i<nInd ; i++)
            {
                d = values[t][i]-mean_pheno[t];
                //cout << " | " << values[t][i] << "-" << mean_pheno[t];
                tmpVar += d*d;
            }
            var_pheno[t] = tmpVar / nInd;
            //cout << " => " << var_pheno[t] << endl;
        }

        // 5.1 - Calculate life events //This part is temporary, //must implement general life events
        vector<double> fitValues;

        calc_fitness_multi_vector(fitValues, values, selTraits, t_species->get_selectMatrix_for_patch(get_abs_pos()));

        // 5.2 - Save global fitness result
        add_index("fitness", fitValues, get_ids(), true);
        /*cout << "FINESSMULT ";
        for(unsigned i = 1; i < fitValues.size(); i++)
        {
            cout << fitValues[i] << " ";
        }
        cout << endl;*/

        // 5.3 - Calc cumulative sum
        // TODO : make a function cumsum
        for(unsigned i = 1; i < fitValues.size(); i++)
        {
            fitValues[i] += fitValues[i-1];
        }
        // Divide by greatest value
        if (fitValues.back() != 0)
        {
            for(double &i: fitValues) i /=  fitValues.back();
        }

        add_index("cumul_fitness", fitValues, get_ids(), false); //Index, Do not sort !
    }
    else // if age under threshold, no index needed. But index structure requires data for whole pops
    {

        vector<double> nullValues(index.size(), 0);
        add_index("fitness", nullValues, get_ids(), true);
        add_index("cumul_fitness", nullValues, get_ids(), false); //Index, Do not sort !

        for(unsigned int i = 0; i < selTraits.size(); ++i)
        {
            if(selTraits[i]->is_indexed())
            {
                string name = selTraits[i]->get_name() + "-add";
                add_index(name, nullValues, get_ids(), true);

                name = selTraits[i]->get_name() + "-pheno";
                add_index(name, nullValues, get_ids(), true); //sorted

                if(t_species->get_assortMating() != "")
                {
                    name = selTraits[i]->get_name() + "-phenoWithE";
                    add_index(name, nullValues, get_ids(), true); //sorted
                }

                name = selTraits[i]->get_name() + "-epsi";
                add_index(name, nullValues, get_ids(), false); //not sorted
            }
        }
    }
}



/* Checks the existence of an individual ndex
 */
bool population::index_exist(string key)
{
    auto got = individual_values.find(key);
    if ( got == individual_values.end() ) return false;
    else return true;
}



/* Returns pointer to the individual index designated by key
 * Returns NULL pointer ifnot found
 */
individual_index * population::get_index(string key)
{
    if(index_exist(key) == 0) return NULL;
    else return individual_values[key];
}




/* Returns pointer to the individual index designated by key
 * fails if individual index if not found
 */
individual_index * population::get_index_safe(string key)
{
    if(index_exist(key) == 0)
    {
        cout << "ERROR: could not load individual index for key " << key << " in pop " << abs << endl;
        cout << "Program will now exit." << endl;
        exit(0);
    }
    else return individual_values[key];
}



/* Adds an index to the population
 * Erases any existing index with the same key
 */
void population::add_index(const string& key, const vector<double>& values,
                           const vector<indId>& idVect, bool sort_values)
{
    //If already exist : delete first
    if (index_exist(key))
    {
        delete individual_values[key];
    }
    individual_values[key] = new individual_index(key, values, idVect, sort_values, true);
    // TODO CARE ABOUT REVERSED INDEX PROPERLY
    return;
}
