#include "svm_nu.h"

/**
 *
 * nu SVM
 *
 */

void svm_nu_regression_c::init(kernel_c* new_kernel, parameters_c* new_parameters){
  svm_c::init(new_kernel,new_parameters);
  lambda_nu = 0;
  sum_alpha_nu = 0;
  qp.m = 2;
  nu = new_parameters->nu;
  epsilon_pos=0;
  epsilon_neg=0;
};


SVMFLOAT svm_nu_regression_c::nabla(const SVMINT i){
  if(is_alpha_neg(i) > 0){
    return( sum[i] - all_ys[i]);
  }
  else{
    return(-sum[i] + all_ys[i]);
  };
};


void svm_nu_regression_c::reset_shrinked(){
  svm_c::reset_shrinked();
  sum_alpha_nu = 0;
};



int svm_nu_regression_c::is_alpha_neg(const SVMINT i){
  // variable i is alpha*
  int result;
  if(all_alphas[i] > 0){
    result = 1;
  }
  else if(all_alphas[i] == 0){
    SVMFLOAT k = sum[i] - all_ys[i] + lambda_eq;
    if(k+lambda_nu+epsilon_neg>=0){
      result = -1;
    }
    else if(-k+lambda_nu+epsilon_pos>=0){
      result = 1;
    }
    else{
      result = 2*((i+at_bound[i])%2)-1;
    };
  }
  else{
    result = -1;
  };
  return result;
};


SVMFLOAT svm_nu_regression_c::lambda(const SVMINT i){
  // size lagrangian multiplier of the active constraint

  SVMFLOAT alpha;
  SVMFLOAT result = 0;

  alpha=all_alphas[i];

  if(alpha>is_zero){
    // alpha*
    if(alpha-Cneg >= - is_zero){
      // upper bound active
      result = -lambda_eq-lambda_nu-nabla(i);
    }
    else{
      result = -abs(nabla(i)+lambda_eq+lambda_nu);
    };
  }
  else if(alpha >= -is_zero){
    // lower bound active
    if(is_alpha_neg(i) > 0){
      result = nabla(i) + lambda_eq+lambda_nu;
    }
    else{
      result = nabla(i)-lambda_eq+lambda_nu;
    };
  }
  else if(alpha+Cpos <= is_zero){
    // upper bound active
    result = lambda_eq -lambda_nu - nabla(i);
  }
  else{
    result = -abs(nabla(i)-lambda_eq+lambda_nu);
  };

  return result;
};


int svm_nu_regression_c::feasible(const SVMINT i){
  // is direction i feasible to minimize the target function
  // (includes which_alpha==0)

  if(at_bound[i] >= shrink_const){ return 0; };

  SVMFLOAT alpha;
  SVMFLOAT result;

  alpha=all_alphas[i];
  //  feasible_epsilon=-1;
  if(alpha-Cneg >= - is_zero){
    // alpha* at upper bound
    result = -lambda_eq-lambda_nu-nabla(i);
    if(result>=-feasible_epsilon){
      return 0; 
    };
  }
  else if((alpha<=is_zero) && (alpha >= -is_zero)){
    // lower bound active
    if(is_alpha_neg(i) > 0){
      result = nabla(i) + lambda_eq+lambda_nu;
    }
    else{
      result = nabla(i)-lambda_eq+lambda_nu;
    };
    if(result>=-feasible_epsilon){
      return 0; 
    };
  }
  else if(alpha+Cpos <= is_zero){
    // alpha at upper bound
    result = lambda_eq -lambda_nu- nabla(i);
    if(result>=-feasible_epsilon){
      return 0; 
    };
  }
  else{
    // not at bound
    result= abs(nabla(i)+is_alpha_neg(i)*lambda_eq+lambda_nu);
    if(result<=feasible_epsilon){
      return 0; 
    };
  };
  return 1;
};


void svm_nu_regression_c::init_optimizer(){
  lambda_nu_WS = 0;
  lambda_nu = 0;
  sum_alpha_nu=0;
  qp.m = 2;
  svm_c::init_optimizer();
  smo.init(parameters->is_zero,parameters->convergence_epsilon,working_set_size*working_set_size*10);
  nu *= parameters->realC;
  epsilon_pos=0;
  epsilon_neg=0;
};


void svm_nu_regression_c::project_to_constraint(){
  SVMFLOAT alpha;
  SVMFLOAT my_alpha_sum=sum_alpha;
  SVMFLOAT my_alpha_nu_sum=sum_alpha_nu-nu;
  SVMINT total_pos = 0;
  SVMINT total_neg = 0;
  for(SVMINT i=0;i<examples_total;i++){
    alpha = all_alphas[i];
    my_alpha_sum += alpha;
    my_alpha_nu_sum += abs(alpha);
    if((alpha>is_zero) && (alpha-Cneg < -is_zero)){
      total_neg++;
    }
    else if((alpha<-is_zero) && (alpha+Cpos > is_zero)){
      total_pos++;
    };
  };

  //project
  if((total_pos > 0) && (total_neg > 0)){
    for(SVMINT i=0;i<examples_total;i++){
      alpha = all_alphas[i];
      if((alpha>is_zero) && (alpha-Cneg < -is_zero)){
	all_alphas[i] -= (my_alpha_sum+my_alpha_nu_sum)/2/total_neg;
      }
      else if((alpha<-is_zero) && (alpha+Cpos > is_zero)){
	all_alphas[i] -= (my_alpha_sum-my_alpha_nu_sum)/2/total_pos;
      };
    };
  };
};


int svm_nu_regression_c::convergence(){
  long time_start = get_time();
  SVMFLOAT pos_sum = 0;
  SVMFLOAT neg_sum = 0;
  SVMINT total=0;
  SVMFLOAT alpha_sum=0;
  SVMFLOAT alpha_nu_sum=0;
  SVMFLOAT alpha=0;
  SVMINT i;
  int result=1;

  // actual convergence-test
  total = 0; alpha_sum=0;

  SVMINT total_pos = 0;
  SVMINT total_neg = 0;

  for(i=0;i<examples_total;i++){
    alpha = all_alphas[i];
    alpha_sum += alpha;
    alpha_nu_sum += abs(alpha);
    if((alpha>is_zero) && (alpha-Cneg < -is_zero)){
      // alpha^* => nabla = lambda_eq + lambda_nu
      neg_sum += nabla(i); //sum[i];
      total_neg++;
    }
    else if((alpha<-is_zero) && (alpha+Cpos > is_zero)){
      // alpha => nabla = -lambda_eq + lambda_nu
      pos_sum += nabla(i); //-sum[i];
      total_pos++;
    };
  };

  if((total_pos>0) && (total_neg > 0)){
    lambda_nu = -(neg_sum/total_neg+pos_sum/total_pos)/2;
    lambda_eq = -(neg_sum/total_neg-pos_sum/total_pos)/2;
    if(target_count>2){
      if(parameters->verbosity>=5){
	cout<<"Re-estimating lambdas from WS"<<endl;
      };
      total_pos=0; total_neg=0;
      pos_sum = 0; neg_sum = 0;
      // estimate lambdas from WS
      for(i=0;i<working_set_size;i++){
	alpha = all_alphas[working_set[i]];
	if((alpha>is_zero) && (alpha-Cneg < -is_zero)){
	  // alpha^* => nabla = lambda_eq + lambda_nu
	  neg_sum += nabla(working_set[i]);
	  total_neg++;	  
	}
	else if((alpha<-is_zero) && (alpha+Cpos > is_zero)){
	  // alpha => nabla = -lambda_eq + lambda_nu
	  pos_sum += nabla(working_set[i]);
	  total_pos++;
	};
      };
      if((total_pos>0) && (total_neg > 0)){
	lambda_nu = -(neg_sum/total_neg+pos_sum/total_pos)/2;
	lambda_eq = -(neg_sum/total_neg-pos_sum/total_pos)/2;
      };
      if(target_count>30){
	i = working_set[target_count%working_set_size];
	if(parameters->verbosity>=5){
	  cout<<"Setting lambdas to nabla("<<i<<")"<<endl;
	};
	if(is_alpha_neg(i) > 0){
	  lambda_eq = -nabla(i);
	}
	else{
	  lambda_eq = nabla(i);
	  };
	lambda_nu=0;
      };
    };

    if(parameters->verbosity>= 4){
      cout<<"lambda_eq = "<<lambda_eq<<endl;
      cout<<"lambda_nu = "<<lambda_nu<<endl;
    };
  }
  else{
    // lambda_eq and lambda_nu are a bit harder to find:
    SVMFLOAT max1=-infinity;
    SVMFLOAT max2=-infinity;
    SVMFLOAT max3=-infinity;
    SVMFLOAT max4=-infinity;
    SVMFLOAT the_nabla;
    for(SVMINT i=0;i<examples_total;i++){
      alpha = all_alphas[i];
      the_nabla = nabla(i);
      if(alpha-Cneg>=-is_zero){
	if(the_nabla>max4) max4 = the_nabla;
      }
      else if(alpha+Cpos<=is_zero){
	if(the_nabla>max3) max3 = the_nabla;
      }
      else if((alpha <= is_zero) && (alpha>=-is_zero)){
	if(is_alpha_neg(i)){
	  if(the_nabla>max1) max1 = the_nabla;
	}
	else{
	  if(the_nabla>max2) max2 = the_nabla;
	};
      };
    };

    //    cout<<"max = ("<<max1<<", "<<max2<<", "<<max3<<", "<<max4<<endl;
    
    if(max1==-infinity) max1=0;
    if(max2==-infinity) max2=0;
    if(max3==-infinity) max3=0;
    if(max4==-infinity) max4=0;
    lambda_eq = (max1+max3-max2-max4)/4;
    lambda_nu = (max1+max2-max3-max4)/4;

    //    cout<<((max1+max3)/2)<<" <= lambda_eq <= "<<(-(max2+max4)/2)<<endl;
    //    cout<<((max1+max2)/2)<<" <= lambda_nu <= "<<(-(max3+max4)/2)<<endl;

    if(parameters->verbosity>= 4){
      cout<<"*** no SVs in convergence(), lambda_eq = "<<lambda_eq<<"."<<endl;
      cout<<"                             lambda_nu = "<<lambda_nu<<"."<<endl;
    };
  };

  // check linear constraint
  if(abs(alpha_sum+sum_alpha) > convergence_epsilon){
    // equality constraint violated
    if(parameters->verbosity>= 3){
      cout<<"alpha_sum "<<alpha_sum<<endl;
      cout<<"sum_alpha "<<sum_alpha<<endl;
      cout<<"No convergence: equality constraint violated: |"<<(alpha_sum+sum_alpha)<<"| >> 0"<<endl;
    };
    project_to_constraint();
    result = 0;  
  };
  // note: original nu is already multiplied by C in init_optimizer
  if(abs(alpha_nu_sum+sum_alpha_nu-nu) > convergence_epsilon){
    // equality constraint violated
    if(parameters->verbosity>= 3){
      cout<<"alpha_nu_sum "<<alpha_nu_sum<<endl;
      cout<<"sum_alpha_nu "<<sum_alpha_nu<<endl;
      cout<<"No convergence: nu-equality constraint violated: |"<<(alpha_nu_sum+sum_alpha_nu-nu)<<"| >> 0"<<endl;
    };
    project_to_constraint();
    result = 0;  
  };

  i=0;
  while((i<examples_total) && (result != 0)){
    if(lambda(i)>=-convergence_epsilon){
      i++;
    }
    else{
      result = 0;
    };
  };

  time_convergence += get_time() - time_start;
  return result;
};


void svm_nu_regression_c::init_working_set(){
  if((((SVMFLOAT)examples_total) * nu) > parameters->realC*(((SVMFLOAT)examples_total) - ((SVMFLOAT)(examples_total%2)))){
    nu = (((SVMFLOAT)examples_total) - ((SVMFLOAT)(examples_total%2))) /
      ((SVMFLOAT)examples_total);
    nu *= parameters->realC;
    nu -= is_zero; // just to make sure
    cout<<"ERROR: nu too large, setting nu = "<<nu<<endl;
  };

  if(examples->initialised_alpha()){
    // check bounds
    project_to_constraint();
  };

  // calculate nu-sum
  sum_alpha_nu=0;
  SVMFLOAT the_nu_sum = 0;
  SVMFLOAT the_sum=0;
  for(SVMINT ni=0;ni<examples_total;ni++){
    the_sum += all_alphas[ni];
    the_nu_sum += abs(all_alphas[ni]);
  };

  if((abs(the_sum) > is_zero) || (abs(the_nu_sum-nu) > is_zero)){
    // set initial feasible point
    // neg alpha: -nu/2n
    // pos alpha:  nu/2p

    for(SVMINT ni=0;ni<examples_total/2;ni++){
      examples->put_alpha(ni,nu/((SVMFLOAT)examples_total));
      examples->put_alpha(examples_total-1-ni,-nu/((SVMFLOAT)examples_total));
    };
    examples->set_initialised_alpha();
  };

  svm_c::init_working_set();
};


void svm_nu_regression_c::shrink(){
  // move shrinked examples to back
  if(to_shrink>examples_total/20){
    SVMINT i;
    SVMINT last_pos=examples_total;
    for(i=0;i<last_pos;i++){
      if(at_bound[i] >= shrink_const){
	// shrink i
	sum_alpha += all_alphas[i];
	sum_alpha_nu += is_alpha_neg(i)*all_alphas[i];
	last_pos--;
	examples->swap(i,last_pos);
	kernel->overwrite(i,last_pos);
	sum[i] = sum[last_pos];
	at_bound[i] = at_bound[last_pos];
      };
    };
    
    examples_total = last_pos;
    kernel->set_examples_size(examples_total);
    
    if(parameters->verbosity>=4){
      cout<<"shrinked to "<<examples_total<<" variables"<<endl;
    };
  };
};


void svm_nu_regression_c::optimize(){
  // optimizer-specific call
  // get time
  long time_start = get_time();

  qp.n = working_set_size;

  SVMINT i;
  SVMINT j;

  // equality constraint
  qp.b[0] = 0;
  qp.b[1] = 0;

  for(i=0;i<working_set_size;i++){
    qp.b[0] += all_alphas[working_set[i]];
    if(qp.A[i] > 0){
      qp.b[1] += all_alphas[working_set[i]];
    }
    else{
      qp.b[1] -= all_alphas[working_set[i]];
    };
  };

  // set initial optimization parameters
  SVMFLOAT new_target=0;
  SVMFLOAT old_target=0;
  SVMFLOAT target_tmp;
  for(i=0;i<working_set_size;i++){
    target_tmp = primal[i]*qp.H[i*working_set_size+i]/2;
    for(j=0;j<i;j++){
      target_tmp+=primal[j]*qp.H[j*working_set_size+i];
    };
    target_tmp+=qp.c[i];
    old_target+=target_tmp*primal[i];
  };

  SVMFLOAT new_constraint_sum=0;
  SVMFLOAT new_nu_constraint_sum=0;
  SVMINT pos_count = 0;
  SVMINT neg_count = 0;

  // optimize
  int KKTerror=1;
  int convError=0;

  if(feasible_epsilon>is_zero){
    smo.set_max_allowed_error(feasible_epsilon);
  };

  int result = smo.smo_solve_const_sum(&qp,primal);
  lambda_WS = smo.get_lambda_eq();
  lambda_nu_WS = smo.get_lambda_nu();

  if(parameters->verbosity>=5){
    cout<<"smo ended with result "<<result<<endl;
    cout<<"lambda_WS = "<<lambda_WS<<endl;
    cout<<"lambda_nu_WS = "<<lambda_nu_WS<<endl;
  };

  // clip
  new_constraint_sum=qp.b[0];
  new_nu_constraint_sum=qp.b[1];
  for(i=0;i<working_set_size;i++){
    // check if at bound
    if(primal[i] <= is_zero){
      // at lower bound
      primal[i] = qp.l[i];
    }
    else if(primal[i] - qp.u[i] >= -is_zero){
      // at upper bound
      primal[i] = qp.u[i];
    };                            
    new_constraint_sum -= qp.A[i]*primal[i];
    new_nu_constraint_sum -= primal[i];
  };

  // enforce equality constraint
  pos_count=0;
  neg_count=0;
  for(i=0;i<working_set_size;i++){
    if((primal[i] < qp.u[i]) && (primal[i] > qp.l[i])){
      if(qp.A[i]>0){
	pos_count++;
      }
      else{
	neg_count++;
      };
    };
  };
  if((pos_count>0) && (neg_count>0)){
    SVMFLOAT pos_add = (new_constraint_sum+new_nu_constraint_sum)/(2*pos_count);
    SVMFLOAT neg_add = (new_constraint_sum-new_nu_constraint_sum)/(2*neg_count);
    //    cout<<"Adding ("<<pos_add<<", "<<neg_add<<")"<<endl;
    for(SVMINT i=0;i<working_set_size;i++){
      if((primal[i] > qp.l[i]) && 
	 (primal[i] < qp.u[i])){
	// real sv
	if(qp.A[i]>0){
	  primal[i] += pos_add;
	}
	else{
	  primal[i] -= neg_add;
	};
      };
    };
  }
  else if((abs(new_constraint_sum)   > is_zero) ||
	  (abs(new_nu_constraint_sum) > is_zero)){
    // error, can't get feasible point
    if(parameters->verbosity>=5){
      cout<<"WARNING: No SVs, constraint_sum = "<<new_constraint_sum<<endl
	  <<"              nu-constraint_sum = "<<new_nu_constraint_sum<<endl;
    };
    old_target = -1e20; 
    convError=1;
  };

  // test descend
  new_target=0;
  for(i=0;i<working_set_size;i++){
    // attention: loqo changes one triangle of H!
    target_tmp = primal[i]*qp.H[i*working_set_size+i]/2;
    for(j=0;j<i;j++){
      target_tmp+=primal[j]*qp.H[j*working_set_size+i];
    };
    target_tmp+=qp.c[i];
    new_target+=target_tmp*primal[i];
  };


  if(new_target < old_target){
    KKTerror = 0;
    //      target_count=0;
    if(parameters->descend < old_target - new_target){
      target_count=0;
    }
    else{
      convError=1;
    };
    if(parameters->verbosity>=5){
      cout<<"descend = "<<old_target-new_target<<"  ("<<old_target<<" --> "<<new_target<<")"<<endl;
    };
  }
  else{
    // nothing we can do
    KKTerror=0;
    convError=1;
    if(parameters->verbosity>=5){
      cout<<"WARNING: no descend ("<<old_target<<" -> "<<new_target<<"), stopping"<<endl;
    };
  };

  if(convError){
    target_count++;
    if(new_target>=old_target){