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


/* 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

    //cout << "NPOPS " << n_pops << endl;
    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;
    //cout << "IU:" << idsUpdate << endl;
    if (pInd == NULL) fail("add_individual: pInd is NULL");
    index[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)
{
    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)
        {
            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()
{

    // First we need the number of ind from all pops of the species
    // No copy here only ref to the vector
    int delta_nInd;
    vector<double> &demo = t_species->get_demo();

    //The the flux matrix of seeds (copy needed, minor perf hit); Sum(fm) == 1
    // TODO: is copy really needed? I don't see why ! Ref coul do !
    vector<double> fm = t_species->get_flux_mat(get_abs_pos(), FEMALE);

    // Calculate number of ind for growth by sum of (nind  * flux matrix)
    double nIndGrowth = 0.0;

    for(unsigned int i = 0; i < fm.size(); i++) nIndGrowth += fm[i] * demo[i];

    // TODO: If we switch to non generational model, we need to take age into account

    // Individuals accounting for cap are those in the population
    double nIndIntra = get_nInd();

    //Floor + random = proba for anything beyond coma
    //cout << "preNew G" << growth << " nIG " <<  nIndGrowth << " nIC " << nIndIntra << " c " << cap << endl;
    float randomGuy = Random();
    delta_nInd = (int)(floor(growth * nIndGrowth * (1 - nIndIntra / cap) + randomGuy));// + ;//test with new logic of pop = 0
    //cout << "delta_nInd" << delta_nInd << endl;


    // Hardcaps, we could differentiate cap and max
    //if(delta_nInd < 0) delta_nInd = 0;//
    if(delta_nInd > cap) delta_nInd = cap;

    targetPop = delta_nInd + nIndIntra;

    if (targetPop > cap) targetPop = cap;
    if (targetPop < 0) targetPop = 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;
    }

    int authorizedFails = 0;
    int fails = 0;
    while(targetPop > newInd && fails <= authorizedFails)
    {
        if (spawn(myMap, m_clock)) // if spawn successfull
        {
            //if (newInd%100==0) cout << "new ind " << newInd << endl;
            newInd++;
            //cout  << "SP" << get_nInd() << " to " << targetPop << endl;
        }
        else
        {
            fails++;
            //cout  << "Fail" << fails << endl;
        }
    }

    //if (newInd == 0)
    //{
    //    // declare patch as empty
    //    myMap.setIsEmpty(t_species, get_abs_pos(), true);
    //} MUST NOT BE DONE HERE OR ELDERS WILL NEVER DIE :OOO
}


/* 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
    return next(begin(index), roll)->first;
}



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



/* 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)
 */
void population::generate_ind(individual& motherRef, individual& fatherRef,
                              mClock &m_clock)
{
    individual * pInd = new individual(motherRef, fatherRef, m_clock, t_species);
    this->add_individual(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)
{
    if (needMotherCut)
    {
        // We used pre-calculated cumulative sum of fitness
        individual_index * pii = get_index_safe("fitness");

        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;
        //cout << endl;

        // 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?
 */
bool 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())
        {
            this->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
        {
            //cout << "assort ON" << endl;
            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]++;
            //cout << "NPOP" << npop << "-" << nb_father_by_pop[npop] << endl;
            //cout << "FS" << father.second << endl;
            individual& rFather = father.first->get_rInd(father.second);
            generate_ind(rMother, rFather, m_clock);

            t_species->addMigration(father.first, abs, father.second);
            t_species->addMigration(mother.first, abs, mother.second);
            return true;
        }
        else
        {
            cout << "Papaoutai" << endl;
            return false;
        }
    }
    else
    {
        cout << "No mother found" << endl;
        exit(0);
        return false;
    }

}
/******************* End reproduction *****************************************/




/* 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());
    int step = m_clock.get_step();
    for(const auto& i: index)
    {
        ages.push_back(step - (i.second)->get_birth());
    }
    add_index("age", ages, get_ids(), true);
    return;
}


/* Deletes all individuals in the range [mini, maxi]
 * TODO : check exact boundaries [[, [] or ]] ?
 */
void population::kill_range(string key, double mini, double maxi)
{
    //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);

    for(indref::iterator it = iStart; it != iEnd; it++)
    {
        destroy_individual(it->second);

    }
    return;
}


/* 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();
    addVals.reserve(get_nInd());

    //For each individual (i = pair<indId, individual*>)
    for(auto i: index)
    {
        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());

    // For each trait
    for(unsigned int i = 0; i < traits.size(); ++i)
    {
        // Calculate additive values of all individuals
        calc_additive_values(values[i], traits[i]);
        // 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;
}






/* Calculates phenotypic value from provided genetic value (that are erased)
 * E is optional, gasdev() is a random local environemental deviation
 * Everything is done in place
 */
/*void population::calc_phenotypic_values(vector<double>& genetic_values, double E = 0.0)
{
    for(double& val: genetic_values)
    {
        val = val + E + gasdev(); // TODO check gasdev() range
    }

    return;
}*/


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

        for(unsigned int vi = 0; vi < values[i].size(); ++vi)
        {
            float eps = 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));
        }

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



        //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
        }
    }
    return;
}




/* Basic multitrait fitness
 */
/*void population::calc_fitness_multi(vector<double>& fitValues,
                                    const vector<vector<double>>& traits_values,
                                    const vector<trait *>& traits)
{
    // Needs at least one trait
    if(traits_values.size() == 0) fail("Error, no trait found.");

    double Z, sum, Zopt, iSel;

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

    // Iterate on individuals
    for(unsigned int i = 0; i < traits_values[0].size(); ++i)
    {
        sum = 0.0;
        // Iterate on each trait
        for(unsigned int t = 0; t < traits.size(); ++t)
        {
            Z = traits_values[t][i];
            Zopt = traits[t]->get_Zopt(abs);
            iSel = traits[t]->get_selInt(abs);
            sum += pow(Z-Zopt, 2) * (1/iSel);
        }

        fitValues.push_back(exp(-sum/2));
    }
    return;
}
//END REV 07/15*/

//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();
    //cout << "TR" << nt << endl;
    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];
        }
        dtheta = z-theta;
        wz = exp(-0.5*(dtheta.transpose())*matOm.inverse()*dtheta);
        meanWz += wz;
        fitValues.push_back(wz);
    }
    //cout << "DBG:wz mean at " << abs << " is " << meanWz/sizePop << endl;
    return;
}


/* General function to calc all traits values of a given specie;
 * The list might change given the age class
 */
void population::calc_traits()
{
    // 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);

    // 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);

    /*cout << "ADDMULT ";
    for(unsigned i = 1; i < values[0].size(); i++)
    {
        cout << values[0][i] << " ";
    }
    cout << endl;*/

    // 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);


    //calc_fitness_multi(fitValues, values, selTraits);
    /*cout << "PHENOMULT ";
    for(unsigned i = 1; i < values[0].size(); i++)
    {
        cout << values[0][i] << " ";
    }
    cout << 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();
    }

    /*cout << "FINESSPOSTNORM ";
    for(unsigned i = 1; i < fitValues.size(); i++)
    {
        cout << fitValues[i] << " ";
    }
    cout << endl;*/

    add_index("cumul_fitness", fitValues, get_ids(), false); //Index, Do not sort !

}



/* 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;
}
