#include "DA.h"
#include <limits>
#include <algorithm>
#ifndef EPSILON
#define EPSILON 0.001
#endif
#include <cmath>
static double s_infini = std::numeric_limits<double>::infinity() ;
using namespace std;

void DualAscent::boucle(vector<double>& u, vector <double> & lambda, bool& BorneSup, const char*x)  const
{
//	lambda = vector<double>(lambda.size(),0);
	int nbCol = cost.size();
	int nbLig = b.size();
	vector<double> q = calculePoids();
	vector<double> L(nbCol, 0);
	double pas = 1;
	double meilleurZd = - s_infini;
	vector<double> meilleursU(u);
	vector<double> meilleursLambda(lambda);
	
	cout << "Cout des colonnes : ";
	for (int c = 0; c < cost.size(); c++)
	{ cout << cost[c] << "\t" ;}cout << endl;
	
	// Recherche d'un borne sup
	BorneSup = testUB();
	double UB;
	
	if (BorneSup)
	{
		UB = recherche_UB();
	}
	
	int nbItMax = 50;
	for ( int nbIt=0 ; nbIt < nbItMax ; nbIt++ )
	{
		u = DAcalc(lambda, L, q);
		vector<double> x(nbCol, 0);
		vector<double> s = sousgrad(x, u, lambda, L, q);
		
		//Calcul de Zd
		double zd = 0;
		double normeS = 0;
		for ( int i=0 ; i < nbLig ; i++ )
		{
#ifdef _DEBUG_LAMBDA
			if (nbIt ==0)
			{
				cout <<"lambda["<<i<<"] = " <<lambda[i]<<" u[i] = "<<u[i] << " b[i] = " << b[i] <<endl; 
			}
#endif
			zd += u[i] * b[i];
			normeS += s[i]*s[i];
		}
		
		//Eventuelle mémorisation si meilleur score
		if ( zd > meilleurZd )
		{
			meilleurZd = zd;
			meilleursU = u;
			meilleursLambda = lambda;
		}
		
		//Maj lambda
		if ( normeS > EPSILON )
		{
			if (BorneSup)
			{
				for ( int i=0 ; i < nbLig ; i++ )
				{
					lambda[i] -= pas * s[i] * (UB -zd) / normeS;
				}
			}
			else
			{
				for ( int i=0 ; i < nbLig ; i++ )
				{
					lambda[i] -= pas * s[i] * 0.1 * abs(zd) / normeS;
				}
			}
		}
#ifdef _DEBUG_ZD
		cout<<"zd = "<<zd<<" à l'itération "<<nbIt<<endl;
#endif
		pas *= 0.99;
	}
	u = meilleursU;
	lambda = meilleursLambda;
}

vector<double> DualAscent::calculePoids() const
{
	vector<double> q(cost.size(), 0);
	for ( int j=0 ; j < cost.size() ; j++ )
	{
		for ( int i=0 ; i < b.size() ; i++ )
		{
			q[j] += a(j, i);
		}
	}
	return q;
}

vector<double> DualAscent::DAcalc(const vector<double> &lambda, vector<double> &L,const vector<double> &q ) const
{
	vector<double> u(b.size(), 0);
	
	//Calcul des L[j]
	for ( int j=0 ; j < L.size() ; j++ )
	{
		L[j] = 0;
		for ( int i=0 ; i < b.size() ; i++ )
		{
			L[j] += a(j, i) * lambda[i];
		}
	}
	
	//Calcul des Ui
	for ( int i=0 ; i < b.size() ; i++ )
	{
		double wmin = s_infini;
		for ( int j=0 ; j < L.size() ; j++ )
		{
			if ( a(j, i) > 0 )
			{
				double w = (cost[j]-L[j])/q[j];
				wmin = (wmin < w) ? wmin : w;
			}
		}
		u[i] = wmin + lambda[i];
	}
	return u;
}

vector<double> DualAscent::sousgrad(vector<double> &x,
                                    const vector<double> &u,
                                    const vector<double> &lambda,
                                    const vector<double> &L,
                                    const vector<double> &q) const
{
	//Calcul de l'indexMin
	vector<int> indexMin(b.size(), -1);
	for ( int i=0 ; i < b.size() ; i++ )
	{
		double mini = s_infini;
		for ( int j=0 ; j < cost.size() ; j++ )
		{
			if ( a(j, i) > 0 )
			{
				double w = (cost[j]-L[j])/q[j];
				if ( mini > w )
				{
					mini = w;
					indexMin[i] = j;
				}
			}
		}
	}
	
	//Calcul de x
		//initialisation
	for ( int j=0 ; j < cost.size() ; j++ )
	{
		x[j] = 0;
	}
		//Remplissage
	for ( int i=0 ; i < indexMin.size() ; i++)
	{
		x[indexMin[i]] += b[i];
	}
		//Division
	for ( int j=0 ; j < cost.size() ; j++ )
	{
		x[j] /= q[j];
	}
	
	//Calcul de s
	vector<double> s(b.size(), 0);
	for ( int i=0 ; i < s.size() ; i++ )
	{
		for ( int j=0 ; j < cost.size() ; j++ )
		{
			s[i] += x[j]*a(j, i);
		}
		s[i] -= b[i];
	}
	
	return s;
}

void DualAscent::vireColonneThermique()
{
    A.erase(A.begin());
    recharge.erase(recharge.begin());
    cost.erase(cost.begin());
    reste.erase(reste.begin());
}

// Test Borne Sup

bool DualAscent::testUB () const
{
    bool out = false;
    bool STOP = false;
    int compteur = 0;
    double SumProd;
    while (!STOP)
    {
        SumProd = 0;
        for (int l =1 ; l < cost.size() ; l++)
        {
         	SumProd+=a(l,compteur);
        }
        
        out = (SumProd>=b[compteur]);
        
        STOP = (!out);
        
        compteur++;
        
        if (compteur==b.size())
        {
            STOP = true;
        }
    }
    if (!(out))
    {
    	cout << "Sumprod : "<< SumProd<< " et b["<<(compteur-1)<<"] = "<<b[compteur]<<endl;
    }
    return out;
}

double DualAscent::recherche_UB () const
{
    double UB = 0;
    
    int n = cost.size();
    int m = b.size();
    
    vector<bool> choix (n,false);
    choix[0] = true ; // la première colonne est la colonne thermique

    vector<double> SumProd (m,0);

    for (int k =  b.size()-1;k>=0;k--)
    {
        if (SumProd[k]>=b[k]) {;}
        else
        {
            bool STOP = false;
            int compteur = 0 , index_min=-1;
 
            double cost_min = s_infini;
 
            while (!STOP)
            {
                if (!choix[compteur])
                {   
                // Colonne non déjà sélectionnée
                    if (a(compteur,k) > 0)
                    {
                    	// Colonne à production positive
                        if (cost[compteur] < cost_min)
                        {
                            index_min=compteur;
                            cost_min=cost[compteur];
                        }
                    }
                }
                
                compteur++;

                if (compteur==n)
                {
                    if (index_min>=0)
                    {
                        choix[index_min]=true;
                        UB += cost_min; 
                        for (int aa=0;aa<m;aa++)
                        {
                            SumProd[aa]+=a(index_min,aa);
                        }
                        compteur = 0 , index_min=-1;
                        cost_min = s_infini;
                    }
                    else
                    {
                        break;
                    }
                    STOP = (SumProd[k]>=b[k]);
                }
            }
        }
    }
    
    cout << "Calcul de borne Sup : "<<UB<<endl;
    for (int t=0;t<n;t++)
    cout << choix[t] << " ";cout << endl;

return UB;
}