% Steepest ascent method
% Written 2006 Markus Triska triska@gmx.at
% Public domain code.

:- use_module(library(simplex)).
:- use_module(library(clpr)).
:- use_module(library(clpfd)).

%?- example(E, Max).
%@ E = a,
%@ Max = [1, 3] ;
%@ E = b,
%@ Max = [1.0, 2.0] ;
%@ E = c,
%@ Max = [0.5, 1.5] ;
%@ false.

example(a, Max) :-
	maximize(- ((x(1) - 2)^2) - (x(2) - 4)^2,
	   [[1,1],[1,-1],[1,0],[-1,0]],
	   [4,0,1,0],
	   [0,0], Max).

example(b, Max) :-
	maximize(-((x(1)-2)^2) - ((x(2) -3)^2),
	   [[1,1],[-1,0]],
	   [3,0],
	   [3,0], Max).

example(c, Max) :-
	maximize(-((x(1)-2)^2) - (x(2)-3)^2,
		 [[1,1],[-1,0],[0,-1]],
		 [2,0,0],
		 [0,0], Max).


% Maximize Obj s.t. Ax =< B, starting position Pos

maximize(Obj, A, B, Pos, Max) :-
	gradient(Pos, 1, Obj, Gs),
	sa(Obj, A, B, Pos, Gs, Max).

sa(Obj, A, B, Pos, Gradient, Max) :-
        % format("currently at: ~w\n", [Pos]),
	active_inactive(A, B, Pos, As, Is),
	maplist(evaluate(Pos), Gradient, GA),
	locally_best(GA, As, Direction),
        % format("locally best direction: ~w\n", [Direction]),
	(   maplist(=(0), Direction) -> Max = Pos
	;   coeffs_sum(GA, Direction, 0) -> Max = Pos
	;   mu_prime1(Is, Pos, Direction, inf, Mu1),
            mu_prime2(Obj, Pos, Direction, Mu2),
            (   Mu2 =:= 0 -> Max = Pos
            ;   (   Mu1 = inf ->
                    Mu = Mu2
                ;   Mu is min(Mu1, Mu2)
                ),
                % format("mu' = ~w, mu'' = ~w   ==>  m = ~w\n\n", [Mu1,Mu2,Mu]),
                maplist(next_pos(Mu), Pos, Direction, Pos1),
                sa(Obj, A, B, Pos1, Gradient, Max)
            )
	).

next_pos(Mu, P, D, P1) :- P1 is P + Mu*D.

mu_prime1([], _, _, Mu, Mu).
mu_prime1([ia(As,B)|Is], Pos, S, Mu0, Mu) :-
	coeffs_sum(As, S, AiS),
	(   AiS > 0 ->
            coeffs_sum(As, Pos, AiX),
            M is (B - AiX) rdiv AiS,
            (   Mu0 = inf -> Mu1 = M
            ;   Mu1 is min(Mu0, M)
            )
	;   Mu1 = Mu0
	),
	mu_prime1(Is, Pos, S, Mu1, Mu).


mu_prime2(Obj, Pos, Direction, Mu) :-
	substitute_variables(Obj, Pos, Direction, Obj1),
	derivative(Obj1, mu, D1),
	replace_mu_by(D1, Mu, D2),
	{ D2 = 0 },
	(   var(Mu) ->
            format("failed to calculate mu''\n"),
            false
	;   true
	).

replace_mu_by(mu, Mu, Mu) :- !.
replace_mu_by(C, _, C) :- number(C), !.
replace_mu_by(-A, Mu, -R) :- !,
	replace_mu_by(A, Mu, R).
replace_mu_by(Expr, Mu, Rep) :-
	Expr =.. [Op,L,R],
	replace_mu_by(L, Mu, L1),
	replace_mu_by(R, Mu, R1),
	Rep =.. [Op,L1,R1].


substitute_variables(O, Pos, Direction, S) :-
	(   number(O) -> S = O
	;   O = -O1,
            substitute_variables(O1, Pos, Direction, SO1),
            S = -SO1
	;   O =.. [Op,A,B] ->
            substitute_variables(A, Pos, Direction, SA),
            substitute_variables(B, Pos, Direction, SB),
            S =.. [Op,SA,SB]
	;   O = x(N) ->
            nth1(N, Pos, P),
            nth1(N, Direction, D),
            S = P + mu*D
	).


locally_best(GA, As, Direction) :-
	gen_state(S0),
	post_constraints(As, S0, S1),
	abs_leq_1(GA, 1, S1, S2),
	gen_constraint(GA, 1, Obj),
	maximize(Obj, S2, Max),
	fetch_direction(GA, 1, Max, Direction).

fetch_direction([], _, _, []).
fetch_direction([_|Rest], N0, S, [D|Ds]) :-
	variable_value(S, sp(N0), Pos),
	variable_value(S, sn(N0), Neg),
	D is Pos - Neg,
	N1 #= N0 + 1,
	fetch_direction(Rest, N1, S, Ds).


abs_leq_1([], _, S, S).
abs_leq_1([_|Rest], N0, S0, S) :-
	constraint([sp(N0)] =< 1, S0, S1),
	constraint([sn(N0)] =< 1, S1, S2),
	N1 #= N0 + 1,
	abs_leq_1(Rest, N1, S2, S).

post_constraints([], S, S).
post_constraints([A|As], S0, S) :-
	gen_constraint(A, 1, C),
	constraint(C =< 0, S0, S1),
	post_constraints(As, S1, S).

gen_constraint([], _, []).
gen_constraint([G|Gs], N0, [G*sp(N0),-G*sn(N0)|Cs]) :-
	N1 #= N0 + 1,
	gen_constraint(Gs, N1, Cs).

gradient([], _, _, []).
gradient([_|Rest], N0, Obj, [G|Gs]) :-
	derivative(Obj, x(N0), G),
	N1 #= N0 + 1,
	gradient(Rest, N1, Obj, Gs).


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

derivative(X, X, 1) :- !.
derivative(C, X, 0) :-
	(   number(C) -> true
	;   C = x(_) -> dif(C, X)
	;   false
	).
derivative(-A, X, -DA) :-
	derivative(A, X, DA).
derivative(A+B, X, DA+DB) :-
	derivative(A, X, DA),
	derivative(B, X, DB).
derivative(A-B, X, DA-DB) :-
	derivative(A, X, DA),
	derivative(B, X, DB).
derivative(C*A, X, C*DA) :- number(C), !, derivative(A, X, DA).
derivative(A*B, X, DA*B + A*DB) :-
	derivative(A, X, DA),
	derivative(B, X, DB).
derivative(A/B, X, D) :- derivative(A*B^(-1), X, D).
derivative(A^C, X, C*A^(C-1)*DA) :-
	number(C),
	derivative(A, X, DA).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

evaluate(Pos, E, V) :-
	(   number(E) -> V = E
	;   E = -E1 ->
            evaluate(Pos, E1, VE1),
            V is - VE1
	;   E =.. [Op,A,B] ->
            evaluate(Pos, A, VA),
            evaluate(Pos, B, VB),
            Eval =.. [Op,VA,VB],
            V is Eval
	;   E = x(N) ->
            nth1(N, Pos, V)
	).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

active_inactive([], _, _, [], []).
active_inactive([Cs|Css], [B|Bs], Pos, As, Is) :-
	coeffs_sum(Cs, Pos, Sum),
	(   Sum =:= B ->
            As = [Cs|AsRest],
            IsRest = Is
	;   AsRest = As,
            Is = [ia(Cs,B)|IsRest]
	),
	active_inactive(Css, Bs, Pos, AsRest, IsRest).


coeffs_sum(Coeffs, Vars, Sum) :-
	foldl(coeffs_sum_, Coeffs, Vars, 0, Sum).

coeffs_sum_(C, P, Sum0, Sum) :- Sum is Sum0 + C*P.