TEST(TestSemiGlobalDev, WithBT)
{    
    tdv::BirchfieldCostDev cost(64);
    tdv::SemiGlobalDev sg;

   runStereoTest(
       "../../res/tsukuba512_L.png",
       "../../res/tsukuba512_R.png",
       "tsukuba_btsgdev.png",
       &cost, &sg, false, false);
}

static void print_range(FILE *fp,tensor P,real MSF,real energy)
{
    int  i;

    fprintf(fp,"%8.3f  %8.3f  %8.3f  %8.3f",
            cost(P,MSF,energy),trace(P)/3,MSF,energy);
    for(i=0; (i<nparm); i++)
        fprintf(fp," %s %10g",esenm[range[i].ptype],range[i].rval);
    fprintf(fp," FF\n");
    fflush(fp);
}

int QuickBucket::GetCase(xyLoc & l1, xyLoc & l2, xyLoc & g)
{
	abCost cost(DeltaCostCD(l1,l2,g));
	for (int i = 0; i < 6; i ++)
	{
		if (cost.a == A[i] && cost.b == B[i])
		{
			return i;
		}
	}
	return 0;
}

void RFrame::print(const char* kind) {
#ifndef PRODUCT
#if defined(COMPILER2) || INCLUDE_JVMCI
  int cnt = top_method()->interpreter_invocation_count();
#else
  int cnt = top_method()->invocation_count();
#endif
  tty->print("%3d %s ", _num, is_interpreted() ? "I" : "C");
  top_method()->print_short_name(tty);
  tty->print_cr(": inv=%5d(%d) cst=%4d", _invocations, cnt, cost());
#endif
}

VectorU cost_grad(PR2System& sys, const VectorJ& j, const Vector3d& obj, const VectorU& u, const double alpha) {
	VectorU grad;

	MatrixX sigma = .01*MatrixX::Identity();
	sigma.block<3,3>(J_DIM,J_DIM) = 20*Matrix3d::Identity();

	double cost_p, cost_m;
	VectorU u_plus = u, u_minus = u;
	for(int i=0; i < U_DIM; ++i) {
		u_plus = u; u_minus = u;
		u_plus(i) = u(i) + epsilon;
		u_minus(i) = u(i) - epsilon;

		cost_p = cost(sys, j, obj, u_plus, alpha);

		cost_m = cost(sys, j, obj, u_minus, alpha);

		grad(i) = (cost_p - cost_m) / (2*epsilon);
	}
	return grad;
}

int main(int, const char * []) {
    IloEnv env;
    try {
        IloInt i,j;
        IloModel model(env);

        IloNumExpr cost(env);
        IloIntervalVarArray allTasks(env);
        IloIntervalVarArray joeTasks(env);
        IloIntervalVarArray jimTasks(env);
        IloIntArray joeLocations(env);
        IloIntArray jimLocations(env);

        MakeHouse(model, cost, allTasks, joeTasks, jimTasks, joeLocations,
                  jimLocations, 0, 0,   120, 100.0);
        MakeHouse(model, cost, allTasks, joeTasks, jimTasks, joeLocations,
                  jimLocations, 1, 0,   212, 100.0);
        MakeHouse(model, cost, allTasks, joeTasks, jimTasks, joeLocations,
                  jimLocations, 2, 151, 304, 100.0);
        MakeHouse(model, cost, allTasks, joeTasks, jimTasks, joeLocations,
                  jimLocations, 3, 59,  181, 200.0);
        MakeHouse(model, cost, allTasks, joeTasks, jimTasks, joeLocations,
                  jimLocations, 4, 243, 425, 100.0);

        IloTransitionDistance tt(env, 5);
        for (i=0; i<5; ++i)
            for (j=0; j<5; ++j)
                tt.setValue(i, j, IloAbs(i-j));

        IloIntervalSequenceVar joe(env, joeTasks, joeLocations, "Joe");
        IloIntervalSequenceVar jim(env, jimTasks, jimLocations, "Jim");

        model.add(IloNoOverlap(env, joe, tt));
        model.add(IloNoOverlap(env, jim, tt));

        model.add(IloMinimize(env, cost));

        /* EXTRACTING THE MODEL AND SOLVING. */
        IloCP cp(model);
        if (cp.solve()) {
            cp.out() << "Solution with objective " << cp.getObjValue() << ":" << std::endl;
            for (i=0; i<allTasks.getSize(); ++i) {
                cp.out() << cp.domain(allTasks[i]) << std::endl;
            }
        } else {
            cp.out() << "No solution found. " << std::endl;
        }
    } catch (IloException& ex) {
        env.out() << "Error: " << ex << std::endl;
    }
    env.end();
    return 0;
}

bool doit()
{
    // min moves needed to transit to goal state
	int sCost = cost(ss);

    // we could need more moves than that ... try up to 50
	for(int limit = sCost; limit <= 50; limit++){
		bool ok= dfs(0, sCost, limit, sr, sc, ss, 0);
		if (ok) return true; 
	}
	return false;
}

 auto update_vertex(LpState& s)
 {
     if (s.cell != start)
     {
         auto minimum = huge();
         for (auto neighbour : valid_neighbours_of(s.cell))
             minimum = min(minimum, (at(neighbour).g + cost()));
         s.r = minimum;
     }
     q.remove(s.cell);
     if (s.g != s.r) q.push(s.cell);
 }

void action_t::consume_resource()
{
  resource_consumed = cost();

  if ( sim -> debug )
    log_t::output( sim, "%s consumes %.1f %s for %s", player -> name(),
                   resource_consumed, util_t::resource_type_string( resource ), name() );

  player -> resource_loss( resource, resource_consumed );

  stats -> consume_resource( resource_consumed );
}

/**
 * Loop principal del algoritmo Simulated Annealing.
 *
 * @instance  :: Instancia del BACP
 * @iter      :: Numero maximo de iteraciones para una temperatura t_current
 * @t_current :: Temperatura inicial
 * @t_min     :: Temperatura minima (eps)
 * @alpha     :: Velocidad de decaimiento de la temperatura
 */
void run(struct bacp_instance *instance, int iter, float t_current, float t_min, float alpha)
{
  unsigned short int i;
  unsigned short int new_cost;
  unsigned short int *s_c      = instance->period;
  unsigned short int old_cost  = cost(instance);
  unsigned short int best_cost = old_cost;
  unsigned short int *s_best   = copy_solution(instance->period, instance->n_courses);

  instance->period = NULL;
  while (t_current > t_min) {
    i = 0;
    while (++i <= iter) {
      if (instance->period) free(instance->period);

      instance->period = s_c;
      instance->period = neighbour(instance);
      new_cost         = cost(instance);

      if (new_cost < old_cost || (exp(-abs(new_cost - old_cost)/t_current) > aceptar())) {
        free(s_c);
        s_c              = instance->period;
        old_cost         = new_cost;
        instance->period = NULL;
      }

      if ( best_cost > new_cost ) {
        if (s_best) free(s_best);
        s_best    = copy_solution(s_c, instance->n_courses);
        best_cost = new_cost;
      }
    }
    t_current = t_current * alpha;
  }

  if (instance->period) free(instance->period);
  if (s_c) free(s_c);

  instance->period = s_best;
}

bool DepthMap::filterPushHypothesis(const int x, const int y, const double d, const double sigmaVal)
{
    if (not isValid(x, y)) return false;
    bool matchFound = false;
    bool hypExist = false;
    for(int h = 0; h < hMax; ++h)
    {
        double & d1 = at(x, y, h);
        double & sigma1 = sigma(x, y, h);
        double & cost1 = cost(x, y, h);
        if( d1 == OUT_OF_RANGE )
        {
            if ( h == 0 )
            {
                pushHypothesis(x, y, d, sigmaVal);
                // if(d<1) cout << "Pushing: x: " << x << " y: " << y << " d: " << d << " s: " << sigmaVal << endl;
                return true;
            }
            break;
        }
        else if( (not matchFound) and match(d1, sigma1, d, sigmaVal) )
        {
            filter(d1, sigma1, d, sigmaVal);
            // if(d1<1)
            // {
            //     cout << "After merge: d1: " << d1 << "  s1: " << sigma1 << endl;
            // }
            // cost1 = max(cost1 - 1.0, 0.0);
            cost1 -= 2*COST_CHANGE;
            cost1 = std::max(cost1, 0.0);
            matchFound = true;
            // std::cout << "Merged " << x << " " << y << " " << h << " " << cost1 << endl;
        }
        else
        {
            cost1 += COST_CHANGE; // increase cost for all hypotheses that did not match
        }
    }
    sortHypStack(x, y);
    // if (matchFound) return true;
    //The program reaches this area if no matches were found
    // else return pushHypothesis(x, y, d, sigmaVal);
    return matchFound;
    // else 
    // {
    //     // cout << "Mismatch: " << x << " " << y << " " << d << " " << sigmaVal << endl;
    //     // cout << "\t Options:" << endl;
    //     // for(int h = 0; h < hMax; ++h)
    //     //     cout << "\t" << at(x, y, h) << " " << sigma(x, y, h) << " " << cost(x, y, h) << endl;
    //     return false;
    // }
}

/**
 * Build a piece of canal.
 * @param tile end tile of stretch-dragging
 * @param flags type of operation
 * @param p1 start tile of stretch-dragging
 * @param p2 waterclass to build. sea and river can only be built in scenario editor
 * @param text unused
 * @return the cost of this operation or an error
 */
CommandCost CmdBuildCanal(TileIndex tile, DoCommandFlag flags, uint32 p1, uint32 p2, const char *text)
{
	WaterClass wc = Extract<WaterClass, 0, 2>(p2);
	if (p1 >= MapSize() || wc == WATER_CLASS_INVALID) return CMD_ERROR;

	/* Outside of the editor you can only build canals, not oceans */
	if (wc != WATER_CLASS_CANAL && _game_mode != GM_EDITOR) return CMD_ERROR;

	TileArea ta(tile, p1);

	/* Outside the editor you can only drag canals, and not areas */
	if (_game_mode != GM_EDITOR && ta.w != 1 && ta.h != 1) return CMD_ERROR;

	CommandCost cost(EXPENSES_CONSTRUCTION);
	TILE_AREA_LOOP(tile, ta) {
		CommandCost ret;

		Slope slope = GetTileSlope(tile, NULL);
		if (slope != SLOPE_FLAT && (wc != WATER_CLASS_RIVER || !IsInclinedSlope(slope))) {
			return_cmd_error(STR_ERROR_FLAT_LAND_REQUIRED);
		}

		/* can't make water of water! */
		if (IsTileType(tile, MP_WATER) && (!IsTileOwner(tile, OWNER_WATER) || wc == WATER_CLASS_SEA)) continue;

		ret = DoCommand(tile, 0, 0, flags | DC_FORCE_CLEAR_TILE, CMD_LANDSCAPE_CLEAR);
		if (ret.Failed()) return ret;
		cost.AddCost(ret);

		if (flags & DC_EXEC) {
			switch (wc) {
				case WATER_CLASS_RIVER:
					MakeRiver(tile, Random());
					break;

				case WATER_CLASS_SEA:
					if (TileHeight(tile) == 0) {
						MakeSea(tile);
						break;
					}
					/* FALL THROUGH */

				default:
					MakeCanal(tile, _current_company, Random());
					break;
			}
			MarkTileDirtyByTile(tile);
			MarkCanalsAndRiversAroundDirty(tile);
		}

		cost.AddCost(_price[PR_BUILD_CANAL]);
	}

void Mutation_Kpoint(double *z, double *fz,
				int N_dimension,
				double *theta, double *grid_points, int grid_size,
				double temp, double scl, double *accpr_pop,
				double *z_new){

	int k_old ;
	int k_new ;
	int i;
	int lab_1 ;
	int lab_2 ;

	double fz_new ;
	double rat ;
	double un ;

	self_adj_index_search(&k_old, *fz, grid_points, grid_size) ;

	/* propose a new solution */

	for ( i=1 ; i<=N_dimension ; i++) z_new[i] = z[i] ;

	/*1st dimension*/
	lab_1 = integerrng(1,N_dimension) ;
	un = normalrng()*scl ;
	z_new[lab_1] += un ;

	/*2nd dimension*/
	if ( (uniformrng()<0.5) && (N_dimension>=2) ){
		lab_2 = integerrng(1,N_dimension-1) ;
		if ( lab_2 >= lab_1 ) lab_2++ ;
		un = normalrng()*scl ;
		z_new[lab_2] += un ;
	}

	fz_new = cost( z_new, N_dimension ) ;

	self_adj_index_search(&k_new, fz_new, grid_points, grid_size) ;

	rat = -theta[k_new] -fz_new/temp +theta[k_old] + *fz/temp  ;

	/* Accept reject */

	*accpr_pop = ( (rat>0.0) ? 1.0 : exp( rat ) ) ;

	un = uniformrng() ;
	if ( *accpr_pop>un ){
		*fz = fz_new;
		for ( i = 1 ; i <= N_dimension ; i++) z[i] = z_new[i] ;
	}

}

int E(int i, int l){
	int j, temp; 
	
	if (i<=l){
		return 0;
	}
	if (l==1){
		return cost(1,i);
	}
	//if (l>1){
		int min=1000000000;
		for(j=1; j<i-1; j++){
			temp = E(j,l-1) + cost(j+1,i);
			//printf("temp= %i \n", temp);
			if(temp<min){
				min = temp;
			}
		}
		return min;
	//}
	
}

double costfunction(){
	int i;
	sumsqerr=0,sumerr0=0,sumerr1=0;
    cost();
    for(i=0;i<(int)total_samples;i++){
        sumsqerr+=(err[i]*err[i]);
        sumerr0+=  err[i];
        sumerr1+= (err[i]*x[i]);
        }
	sumsqerr=sumsqerr/(2.0f*(double)total_samples);
//	printf("\nSumSqerr: %lf Sumerr0 %lf Sumerr1: %lf",sumsqerr,sumerr0,sumerr1);
	return sumsqerr;
}

int check_captures(position src, position dst, board const& board,
                   piece what, OutIter out) {
  piece::color_t const player = what.color();
  board::mask supporters;

  int score = 0;

  if (trap(dst)) {
    supporters = neighbourhood(dst) & board.player(player);
    supporters[src] = false;

    // Check for piece stepping into a trap.
    if (supporters.empty()) {
      *out++ = elementary_step::make_capture(dst, what);
      score -= CAPTURE_COEF * cost(what, src);
    }
  }

  // Check for piece abandoning another one standing on a trap.
  if (neighbourhood(src) & board::mask::TRAPS && !trap(dst)) {
    position const trap =
      (neighbourhood(src) & board::mask::TRAPS).first_set();
    boost::optional<piece> const trapped = board.get(trap);
    if (trapped) {
      piece::color_t const trapped_player = trapped->color();
      supporters = neighbourhood(trap) & board.player(trapped_player);
      supporters[src] = false;

      if (supporters.empty()) {
        *out++ = elementary_step::make_capture(trap, *trapped);
        score +=
          (trapped->color() == what.color() ? -1 : +1) *
          CAPTURE_COEF * cost(*trapped, trap);
      }
    }
  }

  return score;
}

static ERL_NIF_TERM cost_nif(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    double x,y,ret;
    if (!enif_get_double(env, argv[0], &x)) {
		return enif_make_badarg(env);
    }
    if (!enif_get_double(env, argv[1], &y)) {
		return enif_make_badarg(env);
    }

    ret = cost(x,y);
    return enif_make_double(env, ret);
}

void wcCostEstimator::update( const wcMetaVector& meta, const wcFloat& nla )
{
	wcFloat cost0 = cost(meta);
	wcFloat step  = 0;

	iteration++;
	if(unseeded)
	{
		optima_expref = meta;
		unseeded	  = false;
	}


	/// Value Function Update
	Vtp	= Vt;
	rt  = 1-cost0;
	dVt = learning_rate*(rt - Vtp);
	Vt += dVt; 



	/// Predicted Optimal Document Update
	input_layer		.activate(meta);
	hidden1_layer	.activate(input_layer);
	hidden2_layer	.activate(hidden1_layer);
	output_layer	.activate(hidden2_layer);
	optima			= output_layer.output;



	if( training && (RMSE() < WC_ENDOFTRAINING) && (iteration > 5UL) )
	{
		training = false;
	}

	
	if( training /*&& (dVt>0)*/ )
	{
		optima_expref	+= dVt*(meta-optima_expref);
		output_layer	.reweightOutput(meta,			learning_rate);
		hidden2_layer	.reweightHidden(output_layer,	learning_rate);
		hidden1_layer	.reweightHidden(hidden2_layer,	learning_rate);
		input_layer		.reweightHidden(hidden1_layer,	learning_rate);

		output_layer	.reweightSet();
		hidden2_layer	.reweightSet();
		hidden1_layer	.reweightSet();
		input_layer		.reweightSet();
	}
}

uint32 EquipImprove::strengthenEquip(uint32 thisid)
{

	GameItem *item= GlobalItemManager::instance().getItemByThisID(thisid);
	CheckCondition(item,ERR_PARAMS);

	uint32 ret = cost(item);
	if(ret!=ERR_SUCCESS)
	{
		item->updateEquipStrengthenLev();
		return ERR_SUCCESS;
	}
	return ret;
}

/* Function that applies the given algorithm.
 * If the line or column are 0, fill the line or column with 0's
 * If there is a match grab the value from the top left cell and add it cost
 * Else grab the Max value from the left cell or top cell
 * i, the line; j, the column; board, the current board
 */
void processCell(int i, int j, Board board){
	if (i == 0 || j == 0) {
		board->matrix[i][j] = 0;
	} else if (board->vectorHeight[i] == board->vectorWidth[j]) {
		lockCell(i-1, j-1, board);
		board->matrix[i][j] = board->matrix[i - 1][j - 1] + cost(i+j);
		unlockCell(i-1, j-1, board);
	}else{
		lockCell(i-1, j, board);
		board->matrix[i][j] = MAX(board->matrix[i - 1][j], board->matrix[i][j - 1]);
		unlockCell(i-1, j, board);
	}
	unlockCell(i, j, board);
}