#include "ei_simplex.h"
#include "common.h"
#include "lagrange.h"
#include "scpview.h"
#include "lce.h"
#include <time.h>

/*#define LCE_VISUALIZATION*/

LCE* lce_new(SCPView* pv,HeuristicType type,int useLagrange) {

    LCE* lc=NULL;

    /*Check parameters and conditions*/
    assert(pv!=NULL);
    
    /*Create new*/
    lc = (LCE*)malloc(sizeof(LCE));
    assert(lc!=NULL);
    
    /*Assign problem view*/
    lc->problemView=pv;
    
    /*Allocate xd and xdd and previous l-class*/
    lc->xd = (double*)malloc(pv->columns*sizeof(double));
    assert(lc->xd!=NULL);
    
    lc->xdd = (double*)malloc(pv->columns*sizeof(double));
    assert(lc->xdd!=NULL);


    lc->lastIntegerLClass=(double*)malloc(pv->columns*sizeof(double));
    assert(lc->lastIntegerLClass!=NULL);


    /*Widely used row and column masks*/
    lc->rowMask=(unsigned char*)malloc(pv->rows);
    lc->columnMask=(unsigned char*)malloc(pv->columns);

    /*Statistics*/
    lc->stIterationCount=0;
    lc->stLexminRunCount=0;
    lc->stLexminLagrangeImproved=0;
    lc->stLexminNoFeasibleCount=0;
    lc->stGTHRunCount=0;
    lc->stGTHLagrangeImproved=0;
    lc->stGTHFeasibleCount=0;
    lc->stTotalCPUTime=0.0;

    /*Type of Lagrangean heuristic*/
    lc->heuristicType=type;
    lc->useLagrange=useLagrange;

    /*Number of improved classes*/
    lc->stIntegerClassesFound=0;

    /*Return newly created LCE*/
    return lc;
}


int lce_solve(LCE* lc,int upperBound) {
    
    SCPView* view;
    EISimplex* lexmin=NULL;
    LCELexminResult result;
    

    /*Initializing supplementary variables*/
    int j=0;
    int lmcost;
    
    /*Clearing storage memory to avoid uninitialized memory access*/
    memset(lc->xd,0,lc->problemView->columns*sizeof(double));
    memset(lc->xdd,0,lc->problemView->columns*sizeof(double));
    memset(lc->lastIntegerLClass,0,lc->problemView->columns*sizeof(double));

    /*Pre-initialization for faster access*/
    view=lc->problemView;
    
    /*STEP 0 Initialization
     ==================================================================*/

    /*Sort columns according to nonincreasing cost*/
    quick_sort_columns(view,lce_compare,NULL);

    
    /*Find LEXMIN of SCP's relaxation set, it is plain lexmin with
      no cost bounds. It is always integer*/
     
    /*New style*/
    lce_find_initial_lexmin(lc);
    lmcost=lce_calculate_xd_cost(lc);

    /*Copy the lexicographically minimal L-class and set it as
      last integer class*/
    /*Set r, record value, check whether it was improved*/
    if (lmcost<upperBound) {
        lc->r=lmcost;
        lce_save_new_improving_integer_lclass(lc,lc->xd);
        
    }
    else lc->r=upperBound;

    /*Check whether solution to SCP was found, it should always happen*/
    assert(scp_view_is_solution_d(lc->problemView,lc->xd)==1);

    /*Set p, max. column such that its value is one, last column excluded*/
    lc->p=-1;
    for(j=view->columns-2;j>-1;j--) {
        if (lc->xd[j]==1) {
            lc->p=j;
            break;
        }
    }
    
    /*If such p does not exist, lexicographic minimum is also perfect
     *solution, L-Class enumeration is finished
     */
    if (lc->p==-1) goto step4;

    
    
    /* STEP 1 Find fi, max. column such that its value is zero, index is to
     * be smaller than p
     * ====================================================================
     */
    step1:
    
    lc->fi=-1;
    for(j=lc->p-1;j>-1;j--) {
        if (lc->xd[j]==0) {
            lc->fi=j;
            break;
        }
    }

    /* If such fi is nonexistent, enumeration is finished*/
    if (lc->fi==-1) goto step4;
    
    /*STEP 2 Solve the linear subproblem:
     *Find xd is lexmin of the SCP relaxation set and its maximum cost
     *is <=r-1. xd elements with index lesser than fi remain intact, but
     *element with index fi is forced to be one
     *======================================================================
     */
    step2:
    

    /*This is next iteration*/
    lc->stIterationCount++;
    
    /*Set xdd=xd*/
    for(j=0;j<view->columns;j++) {
        lc->xdd[j]=lc->xd[j];
    }
    /*Forced one at index fi*/
    lc->xdd[lc->fi]=1;


    /* Now we know that solution to the SCP will be in the following form
     * x = {a,b,c,d,e,...,1,?,?,?,?,?,...,?,?}, so we know that some columns
     * will be part of solution, some will not. Other columns have to be
     * calculated by simplex method
     *
     * We need a solution with cost lower than r, so we will use lagrangean
     * heuristic to test whether solution with such cost is available. Lagrangean
     * heuristic also provide upper bound that can be taken into account
     */
    

    /* STEP 2 TRY TO FIND ANOTHER L-CLASS
     * ====================================================================
     */
   
    lc->stLexminRunCount++;
    result=lce_perform_step_2(lc);
    

    /* STEP 3 EVALUATE RESULTS OF THE SEARCH FOR L-CLASS
     * ====================================================================
     */
  
    switch(result) {
        
        case LCE_LM_QUIT:{
            goto step4;break;
        }

        case LCE_LM_FOUND_INTEGER_L_CLASS: {
            for(j=view->columns-2;j>=0;j--) {
                if (lc->xd[j]==1) {
                    lc->p=j;break;
                }
            }
            goto step1;
            break;
        }
        case LCE_LM_FOUND_L_CLASS: {
            for(j=0;j<view->columns;j++) {
                if (lc->xd[j]!=floor(lc->xd[j])) {
                   lc->fi=j;
                    break;
                }
            }
            goto step2;
            break;
        }

        case LCE_LM_NOT_FOUND: {
            lc->p=lc->fi;
            lc->stLexminNoFeasibleCount++;
            goto step1;
            break;
        }
    }
    
    
    /* STEP 4 Enumeration is finished
     * =====================================================================
     */
    step4:

    /*Delete supplementary solvers*/
    if (lexmin!=NULL) ei_simplex_delete(lexmin);

    return LCE_OK;
}
 

double lce_calculate_xd_cost(LCE* lc) {

    int j=0;
    double cost=0.0;

    for(j=0;j<lc->problemView->columns;j++) {
        cost+=lc->xd[j]*scp_view_get_cost(lc->problemView,j);
    }

    return cost;
}

void lce_get_solution(LCE* lc,unsigned char* retMask) {

    int j=0;

    /*Check parameters*/
    assert(lc!=NULL);
    assert(retMask!=NULL);

    /*Return last integer L-class found*/
    for(j=0;j<lc->problemView->columns;j++) {
        retMask[j]=(lc->lastIntegerLClass[j]==1) ? 1 : 0;
    }


    /*If last integer L-class is not solution, something went wrong*/
    assert(scp_view_is_solution(lc->problemView,retMask)==1);
}



LCELexminResult lce_group_testing_heurisitc(LCE* lc) {

    EISimplex* simplex=NULL;
    LagrangeSolver* lgs=NULL;
    SCPView* view=NULL;
    SCPView* subView=NULL;
    SCPView* lmView=NULL;
    SCPView* lgView=NULL;

    int q=0;
    int p=0;
    int j=0;
    int i=0;
    int fcc=0;
    double val;
    LCELexminResult result;
    int lowBound=0;
    int upBound;
    
    int qr;
    int cost;

    
   
    /*Statistics update*/
    lc->stGTHRunCount++;

    /*Now we are going to create reduced version of SCP*/
    view=lc->problemView;
    
    /*Start at index fi-1. Search for run of ones while
     *decreasing index
     */
    q=lc->fi-1;
    while(q>=0 && (lc->xdd[q]==1)) {
        q--;
    }

    /*No advantage can be taken using group testing heuristic*/
    if (q==lc->fi-1 || q<0) {
        result=LCE_LM_NOT_FOUND;
        goto done;
    }

    /*q now holds index of first zero found*/
    p=q;
    while(p>=0 && lc->xdd[p]==0) {
        p--;
    }

    p++;    /*p is now j0, first zero of the zero run*/
    q++;    /*q now points to first one*/

    /*Explaining the situation
    xdd.110101|000000000000|1111111.....
        fixed  p   al1      q     f
    */

    /*FIXCHECK:  **************************************************************/


     /*Check whether fixed columns do not implicate solution with (c,x)>r-1*/
     for(j=0;j<p;j++) fcc+=(lc->xdd[j]==1)?(scp_view_get_cost(lc->problemView,j)):0;
     if (fcc>lc->r-1) {
         result=LCE_LM_NOT_FOUND;
         goto done;
     }

     /*RESTCHECK: **************************************************************/

     /*Create subproblem that contain only columns that are not fixed and 
      *remove rows covered by fixed columns
      */
      memset(lc->columnMask,1,lc->problemView->columns);    /*All cols*/
      memset(lc->rowMask,1,lc->problemView->rows);          /*All rows*/
      memset(lc->columnMask,0,p);    /*Columns with index lower than p will be removed*/

     /*Removing rows covered by columns in the mask*/
     for(i=0;i<p;i++) {
        if (lc->xdd[i]==0) continue;
        for(j=0;j<lc->problemView->rows;j++) {
            val=scp_view_get(lc->problemView,j,i);
            if (val==1) lc->rowMask[j]=0;
        }
     }

    /*Now we create a view to the subproblem*/
    subView = scp_view_get_row_and_column_mask_subview(lc->problemView,lc->rowMask,lc->columnMask,1);

    /*If subview is null we try to find  L-CLASS with cost<=r-1 that
     *contains only trainling zeroes and one 1 at lexicographically minimal
     *position in 'at least one one' vector segment*/
    if (subView==NULL) {
        cost=scp_view_get_cost(view,q-1);
        if (fcc+cost<=lc->r-1) {
            for(j=p;j<view->columns;j++) lc->xd[j]=0;
            lc->xd[q-1]=1;
            lc->r=fcc+cost;
            lce_save_new_improving_integer_lclass(lc,lc->xd);
            result=LCE_LM_FOUND_INTEGER_L_CLASS;
            goto done;
        }

    }

    /*scp_view_remove_dominance(&subView);*/
    lmView=scp_view_get_copy(subView);
    upBound=lc->r-1;
    
    

    /*LAGRANGE STAGE: *******************************************************/

    if (lc->useLagrange==1) {

        /*If subView is not solvable in domain of integers, we continue to lexmin stage
          passing by the lagrangean heuristic*/
        if (scp_view_check_solvable(subView)==0) goto lexmin_stage;

        /*Lagrangean heuristic has its own view to the subproblem*/
        lgView=scp_view_get_copy(subView);
    
        /*We try to find solution and lower bound using lagrangean heuristic*/
        /*Extending the problem view*/
        scp_view_modify_for_gth(lgView,p,q-1);
        

        lgs=lagrangesolver_new(lgView,lc->heuristicType);
        lagrangesolver_solve(lgs,lc->r-1-fcc,0);

        lowBound=fcc+lgs->ZMAX;
        if (lc->r-1 > upBound) {
            upBound=fcc+lgs->ZUB;
            lc->stGTHLagrangeImproved++;
        }

        /*Cancelling view extension*/
        scp_view_demodify_for_gth(lgView);
    }

 
    lexmin_stage: /************************************************************/

    /*Looking for better L-CLASS using dual simplex method. If lower bound says
     *that it is impossible, we go to lcnotfound*/
     if (lowBound>lc->r-1) {
         result=LCE_LM_NOT_FOUND;
         goto done;
     };

    /*In other case we use simplex method*/
    simplex=ei_simplex_new();
    
    qr=ei_simplex_solve_lexmin(simplex,lmView,upBound-fcc,q-p);

    /*No feasible solution found*/
    if (qr!=EI_SIMPLEX_OPTIMUM) {
        result=LCE_LM_NOT_FOUND;
        goto done;
    }

    /*Simplex method found some L-CLASS*/
    for(i=p;i<view->columns;i++) lc->xd[i]=0.0;
    for(j=0;j<lmView->columns;j++) {
        ei_simplex_get_rhs(simplex,j,&val);
        lc->xd[subView->parentColumnMap[lmView->parentColumnMap[j]]]=val;
    }
    result=LCE_LM_FOUND_L_CLASS;

    /*If this L-CLASS is integer and improves record, it is stored as
     *best integer L-class
     */
    i=0;
    for (j=0;j<lc->problemView->columns;j++) {
        if (lc->xd[j]==floor(lc->xd[j])) i++;
    }

    if (i==lc->problemView->columns) {
        if ((cost=lce_calculate_xd_cost(lc))<lc->r) {
            lc->r=cost;
            lce_save_new_improving_integer_lclass(lc,lc->xd);
        }
        result=LCE_LM_FOUND_INTEGER_L_CLASS;
    }


    done:  /******************************************************************/

    
    if (subView!=NULL) scp_view_delete(subView);
    if (lmView!=NULL) scp_view_delete(lmView);
    if (lgView!=NULL) scp_view_delete(lgView);
    if (simplex!=NULL) ei_simplex_delete(simplex);
    if (lgs!=NULL) lagrangesolver_delete(lgs);
    
    return result;

}

void lce_find_initial_lexmin(LCE* lc) {
    
    int* colCounts=NULL;
    int i,j;
    int c,r;
   
    c=lc->problemView->columns;
    r=lc->problemView->rows;

    colCounts=(int*)malloc(sizeof(int)*r);
    assert(colCounts!=NULL);

    for(i=0;i<r;i++) {
        colCounts[i]=scp_view_get_columns_covering(lc->problemView,i);
    }
  
    /*For each column*/
    for(i=0;i<c;i++) {

        lc->xd[i]=0;

        /*Look for rows, that the column covers*/
        for(j=0;j<r;j++) {
            /*If the row is covered by only the column, this column must
             *remain in the solution*/
            if (scp_view_get(lc->problemView,j,i)==1 && colCounts[j]==1) {
                lc->xd[i]=1;
                break;
            }
        }

        /*Column is removed from the solution, colCounts of rows that
         *are covered by the column are decreased
         */
        if (lc->xd[i]==0) {
            for(j=0;j<r;j++) {
                if (scp_view_get(lc->problemView,j,i)==1) colCounts[j]--;
            }
        }
        
    }

    /*Lexicographic minimum is now stored in the xd vector*/
    free(colCounts);

}

int lce_compare(SCPView* v,int c1,int c2,void* miscData) {

    int v1;
    int v2;
    v1=scp_view_get_cost(v,c1);
    v2=scp_view_get_cost(v,c2);

    if (v1>v2) return -1; else if (v1<v2) return +1;
    return 0;

}

int lce_compare_lclasses_lex(LCE* lc,double* a,double* b) {
    
    int m=lc->problemView->columns;
    int i=0;

    for(i=0;i<m;i++) {
        if (a[i]<b[i]) return -1;
        if (a[i]>b[i]) return +1;
    }
    return 0;

}

 /** Performing step 2*/
 LCELexminResult lce_perform_step_2(LCE* lc) {
     
     SCPView* subView=NULL;
     SCPView* lmView=NULL;
     SCPView* lgView=NULL;
     SCPView* view=NULL;
     
     LagrangeSolver* lgs=NULL;
     EISimplex* simplex=NULL;
     LCELexminResult result;

     int cost=0;
     int lowBound=0,upBound=0;
     int fcc=0;
     int j=0,i=0,qr=0;
     double val;
     int lgUpBound=0;
     

     
     view=lc->problemView;

    #ifdef LCE_VISUALIZATION

       for(i=0;i<lc->fi;i++) {
            printf("%d",(int)lc->xdd[i]);
        }
        printf("f");
        for(i=lc->fi+1;i<lc->p;i++) printf("%d",(int)lc->xdd[i]);
        printf("p");
        for(i=lc->p+1;i<view->columns;i++) printf("?");
        printf("\n");
    
     
    #endif


     /*FIXCHECK:  **************************************************************/

     /*Check whether fixed columns do not implicate solution with (c,x)>r-1*/
     for(j=0;j<lc->fi+1;j++) fcc+=(lc->xdd[j]==1)?(scp_view_get_cost(lc->problemView,j)):0;
     if (fcc>lc->r-1) goto lcnotfound;

     /*RESTCHECK: **************************************************************/
       
     /*Create subproblem that contain only columns that are not fixed and rows 
      *not covered by fixed columns
      */

      
      memset(lc->columnMask,1,lc->problemView->columns);    /*All cols*/
      memset(lc->rowMask,1,lc->problemView->rows);          /*All rows*/
      memset(lc->columnMask,0,lc->fi+1);    /*Columns up to fi removed*/

     /*Removing rows covered by columns in the mask*/
     for(i=0;i<lc->fi+1;i++) {
        if (lc->xdd[i]==0) continue;
        for(j=0;j<lc->problemView->rows;j++) {
            val=scp_view_get(lc->problemView,j,i);
            if (val==1) lc->rowMask[j]=0;
        }
     }
      

    /*No we have obtained that subproblem*/
    subView = scp_view_get_row_and_column_mask_subview(lc->problemView,lc->rowMask,lc->columnMask,1);
    

    /*If subview is null we have found L-CLASS that contains only trailing zeroes*/
    if (subView==NULL) {
        lc->xd[lc->fi]=1;
        for(j=lc->fi+1;j<view->columns;j++) lc->xd[j]=0;
        
        /*This L-CLASS is always better than last integer class, because of testing at
         *fixcheck label, so we update record and we also store it as best integer class*/
        lc->r=fcc;
        lce_save_new_improving_integer_lclass(lc,lc->xd);

        result=LCE_LM_FOUND_INTEGER_L_CLASS;
        goto done;

    }

    lmView=scp_view_get_copy(subView);
    upBound=lc->r-1;
    
    

    /*LAGRANGE STAGE: *********************************************************/

    if (lc->useLagrange) {

        /*If subView is not solvable, we continue to lexmin stage*/
         if (scp_view_check_solvable(subView)==0) goto lexmin_stage;

        /*We try to find solution and lower bound using lagrangean heuristic*/
        lgView=scp_view_get_copy(subView);
        scp_view_remove_dominance(&lgView);
    
        lgs=lagrangesolver_new(lgView,lc->heuristicType);
        lagrangesolver_solve(lgs,lc->r-1-fcc,0);

        lowBound=fcc+lgs->ZMAX;
        lgUpBound=fcc+lgs->ZUB;

    
        if (lgUpBound<upBound) {
            upBound=lgUpBound;
            lc->stLexminLagrangeImproved++;
        }
    }

    lexmin_stage: /************************************************************/
    
    /*Looking for better L-CLASS using simplex method. If lower bound says
     *that it is impossible we go to lcnotfound*/
     if (lowBound>lc->r-1) goto lcnotfound;

    /*In other case we use simplex method*/
    simplex=ei_simplex_new();
    qr=ei_simplex_solve_lexmin(simplex,lmView,upBound-fcc,-1);

    if (qr!=EI_SIMPLEX_OPTIMUM) {
        goto lcnotfound;
    }

    /*Simplex found some L-CLASS*/
    for(i=lc->fi+1;i<view->columns;i++) lc->xd[i]=0.0;

    
    lc->xd[lc->fi]=1;   /*Forced one is part of solution*/
    
    for(j=0;j<lmView->columns;j++) {
        ei_simplex_get_rhs(simplex,j,&val);
        lc->xd[subView->parentColumnMap[lmView->parentColumnMap[j]]]=val;
    }
    

    result=LCE_LM_FOUND_L_CLASS;

    /*If this L-CLASS is integer and improving, it will become best integer
     *L-CLASS
     */
    i=0;
    for (j=0;j<lc->problemView->columns;j++) {
        if (lc->xd[j]==floor(lc->xd[j])) i++;
    }
    
    if (i==lc->problemView->columns) {
        if ((cost=lce_calculate_xd_cost(lc))<lc->r) {
            lc->r=cost;
            lce_save_new_improving_integer_lclass(lc,lc->xd);
        }
        result=LCE_LM_FOUND_INTEGER_L_CLASS;
    }

    goto done;

    lcnotfound:  /*************************************************************/
    
    if (lc->fi==1) {
        result=LCE_LM_QUIT;
    }
    else {
        /*We try to perform group testing heuristic*/
        result=lce_group_testing_heurisitc(lc);
        if (result!=LCE_LM_NOT_FOUND) lc->stGTHFeasibleCount++;
    }


    done:  /******************************************************************/

    if (subView!=NULL) scp_view_delete(subView);
    if (lmView!=NULL) scp_view_delete(lmView);
    if (lgView!=NULL) scp_view_delete(lgView);
    if (simplex!=NULL) ei_simplex_delete(simplex);
    if (lgs!=NULL) lagrangesolver_delete(lgs);
    

    return result;

 }

 void lce_save_new_improving_integer_lclass(LCE* lc,double* mask) {

     assert(mask!=NULL);
     assert(lce_compare_lclasses_lex(lc,mask,lc->lastIntegerLClass)==1);

     memcpy(lc->lastIntegerLClass,mask,lc->problemView->columns*sizeof(double));
     lc->stIntegerClassesFound++;

     

    /*Check that class is a solution*/
     assert(scp_view_is_solution_d(lc->problemView,lc->lastIntegerLClass)==1);

     
 }

void lce_delete(LCE* lc) {

     if (lc==NULL) return;
     if (lc->xd!=NULL)  free(lc->xd);
     if (lc->xdd!=NULL) free(lc->xdd);
     if (lc->lastIntegerLClass!=NULL) free(lc->lastIntegerLClass);
     if (lc->rowMask!=NULL) free(lc->rowMask);
     if (lc->columnMask!=NULL) free(lc->columnMask);

     free(lc);
 }


 void lce_print(LCE* lc) {
     int j=0;

     printf("[LCE] {\n");
     if (lc==NULL) {
         printf("!!! NULL !!!\n}\n");
         return;
     }
     /*scp_view_print(lc->problemView,0);*/

     printf("xd:\n");
     for(j=0;j<lc->problemView->columns;j++) {
         printf("%1.2f,",lc->xd[j]);
     }
     printf("\nxdd:\n");
     for(j=0;j<lc->problemView->columns;j++) {
         printf("%1.2f,",lc->xdd[j]);
     }
     printf("\n");
     printf("fi:%d, p:%d, r:%d\n",lc->fi,lc->p,lc->r);
     printf("}\n");
 }
 
 
