import numpy as np
import math
from scipy.optimize import show_options

#% ======================================
#% ========= begin subfunctions =========
#% ======================================
def intrafun(x,params): #function fval = intrafun(x,params)
    #% transform variables, then call original function

    #% transform
    xtrans = xtransform(x,params)

    #% and call fun
    fval = params['fun'](np.reshape(xtrans,params['xsize']),params['args']) #fval = feval(params.fun,reshape(xtrans,params.xsize),params.args{:});

    #end % sub function intrafun end

    #% ======================================
    return (fval)

def xtransform(x,params): #function xtrans = xtransform(x,params)
    #% converts unconstrained variables into their original domains

    xtrans = np.zeros(params['xsize']) #xtrans = zeros(params.xsize);
    #% k allows some variables to be fixed, thus dropped from the
    #% optimization.
    k=1 #k=1;
    for i in range(params['n']): #i = 1:params.n
        match params['BoundClass'][i]: #switch params.BoundClass(i)
            case 1:
                #% lower bound only
                xtrans[i] = params['LB'][i] + x[k]*x[k] #xtrans(i) = params.LB(i) + x(k).^2;
      
                k=k+1 #k=k+1;
            case 2:
                #% upper bound only
                xtrans[i] = params['UB'][i] - x[k]*x[k] #xtrans(i) = params.UB(i) - x(k).^2;
      
                k=k+1 #k=k+1;
            case 3:
                #% lower and upper bounds
                xtrans[i] = (np.sin(x[k])+1)/2 #xtrans(i) = (sin(x(k))+1)/2;
                xtrans[i] = xtrans[i]*(params['UB'][i] - params['LB'][i]) + params['LB'][i]#xtrans(i) = xtrans(i)*(params.UB(i) - params.LB(i)) + params.LB(i);
                #% just in case of any floating point problems
                xtrans[i] = max(params['LB'][i],min(params['UB'][i],xtrans[i]))#xtrans(i) = max(params.LB(i),min(params.UB(i),xtrans(i)));
      
                k = k+1
            case 4:
                #% fixed variable, bounds are equal, set it at either bound
                xtrans[i] = params['LB'][i]
            case 0:
                #% unconstrained variable.
                xtrans[i] = x[k] #xtrans(i) = x(k);
      
                k=k+1
            #end
        #end
    return(xtrans)
    #end % sub function xtransform end

def fminsearchbnd(fun,x0,LB,UB,options,varargin): #function [x,fval,exitflag,output] = fminsearchbnd(fun,x0,LB,UB,options,varargin)
    #% FMINSEARCHBND: FMINSEARCH, but with bound constraints by transformation
    #% usage: x=FMINSEARCHBND(fun,x0)
    #% usage: x=FMINSEARCHBND(fun,x0,LB)
    #% usage: x=FMINSEARCHBND(fun,x0,LB,UB)
    #% usage: x=FMINSEARCHBND(fun,x0,LB,UB,options)
    #% usage: x=FMINSEARCHBND(fun,x0,LB,UB,options,p1,p2,...)
    #% usage: [x,fval,exitflag,output]=FMINSEARCHBND(fun,x0,...)
    #% 
    #% arguments:
    #%  fun, x0, options - see the help for FMINSEARCH
    #%
    #%  LB - lower bound vector or array, must be the same size as x0
    #%
    #%       If no lower bounds exist for one of the variables, then
    #%       supply -inf for that variable.
    #%
    #%       If no lower bounds at all, then LB may be left empty.
    #%
    #%       Variables may be fixed in value by setting the corresponding
    #%       lower and upper bounds to exactly the same value.
    #%
    #%  UB - upper bound vector or array, must be the same size as x0
    #%
    #%       If no upper bounds exist for one of the variables, then
    #%       supply +inf for that variable.
    #%
    #%       If no upper bounds at all, then UB may be left empty.
    #%
    #%       Variables may be fixed in value by setting the corresponding
    #%       lower and upper bounds to exactly the same value.
    #%
    #% Notes:
    #%
    #%  If options is supplied, then TolX will apply to the transformed
    #%  variables. All other FMINSEARCH parameters should be unaffected.
    #%
    #%  Variables which are constrained by both a lower and an upper
    #%  bound will use a sin transformation. Those constrained by
    #%  only a lower or an upper bound will use a quadratic
    #%  transformation, and unconstrained variables will be left alone.
    #%
    #%  Variables may be fixed by setting their respective bounds equal.
    #%  In this case, the problem will be reduced in size for FMINSEARCH.
    #%
    #%  The bounds are inclusive inequalities, which admit the
    #%  boundary values themselves, but will not permit ANY function
    #%  evaluations outside the bounds. These constraints are strictly
    #%  followed.
    #%
    #%  If your problem has an EXCLUSIVE (strict) constraint which will
    #%  not admit evaluation at the bound itself, then you must provide
    #%  a slightly offset bound. An example of this is a function which
    #%  contains the log of one of its parameters. If you constrain the
    #%  variable to have a lower bound of zero, then FMINSEARCHBND may
    #%  try to evaluate the function exactly at zero.
    #%
    #%
    #% Example usage:
    #% rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
    #%
    #% fminsearch(rosen,[3 3])     % unconstrained
    #% ans =
    #%    1.0000    1.0000
    #%
    #% fminsearchbnd(rosen,[3 3],[2 2],[])     % constrained
    #% ans =
    #%    2.0000    4.0000
    #%
    #% See test_main.m for other examples of use.
    #%
    #%
    #% See also: fminsearch, fminspleas
    #%
    #%
    #% Author: John D'Errico
    #% E-mail: woodchips@rochester.rr.com
    #% Release: 4
    #% Release date: 7/23/06

    #% size checks
    xsize = np.size(x0)
    x0 = x0[:]
    n = len(x0)

    if (nargin<3) or len(LB)==0: #(nargin<3) || isempty(LB)
        LB = np.tile(-math.inf,n,1) #LB = repmat(-inf,n,1)
    else:
        LB = LB[:]
    #end
    if (nargin<4) or len(UB)==0: #(nargin<4) || isempty(UB)
       UB = np.tile(math.inf,n,1)
    else:
      UB = UB[:]
    #end

    if (n!=len(LB)) or (n !=len(UB)):
      raise ValueError("x0 is incompatible in size with either LB or UB.")
    #end

    #% set default options if necessary
    if (nargin<5) or len(options)==0:
        show_options(solver="minimize") #options = optimset('fminsearch')
    #end

    #% stuff into a struct to pass around
    params = []    
    params['args'] = varargin
    params['LB'] = LB #params.LB = LB;
    params['UB'] = UB #params.UB = UB;
    params['fun'] = fun #params.fun = fun;
    params['n'] = n#params.n = n;
    #% note that the number of parameters may actually vary if 
    #% a user has chosen to fix one or more parameters
    params['xsize'] = xsize #params.xsize = xsize;
    params['OutputFcn'] = []#params.OutputFcn = [];

    #% 0 --> unconstrained variable
    #% 1 --> lower bound only
    #% 2 --> upper bound only
    #% 3 --> dual finite bounds
    #% 4 --> fixed variable
    params['BoundClass'] = np.zeros[n,1] #params.BoundClass = zeros(n,1);
    for i in range(n): #i=1:n
        k = np.isfinite(LB[i]) + 2*np.isfinite(UB[i]) #k = isfinite(LB(i)) + 2*isfinite(UB(i));
        params['BoundClass'][i] = k #params.BoundClass(i) = k;
        if (k==3) and (LB[i]==UB[i]):#if (k==3) && (LB(i)==UB(i))
            params['BoundClass'][i] = 4 #params.BoundClass(i) = 4;
        #end
    #end

    #% transform starting values into their unconstrained
    #% surrogates. Check for infeasible starting guesses.
    x0u = x0 #x0u = x0;
    k=1 #k=1;
    for i in range(n): # i = 1:n
        match params['BoundClass'][i]: #switch params.BoundClass(i)
            case 1:
                #% lower bound only
                if x0[i]<=LB[i]: #x0(i)<=LB(i)
                    #% infeasible starting value. Use bound.
                    x0u[k] = 0
                else:
                    x0u[k] = np.sqrt(x0[i] - LB[i]) #x0u(k) = sqrt(x0(i) - LB(i));
                #end
                #% increment k
                k = k+1 #k=k+1;
            case 2:
                #% upper bound only
                if x0[i]>=UB[i]: #x0(i)>=UB(i):
                    #% infeasible starting value. use bound.
                    x0u[k] = 0 #x0u(k) = 0;
                else:
                    x0u[k] = np.sqrt(UB[i] - x0[i]) #x0u(k) = sqrt(UB(i) - x0(i));
                #end
      
                #% increment k
                k=k+1 #k=k+1;
            case 3:
                #% lower and upper bounds
                if x0[i] <= LB[i]: #x0(i)<=LB(i)
                    #% infeasible starting value
                    x0u[k] = -np.pi/2 #x0u(k) = -pi/2;
                elif x0[i] >= UB[i] : #x0(i)>=UB(i)
                    #% infeasible starting value
                    x0u[k] = np.pi/2 #x0u(k) = pi/2;
                else:
                    x0u[k] = 2*(x0[i] - LB[i])/(UB[i]-LB[i]) - 1#x0u(k) = 2*(x0(i) - LB(i))/(UB(i)-LB(i)) - 1;
                    #% shift by 2*pi to avoid problems at zero in fminsearch
                    #% otherwise, the initial simplex is vanishingly small
                    x0u[k] = 2*np.pi+np.asin(max(-1,min(1,x0u[k]))) #x0u(k) = 2*pi+asin(max(-1,min(1,x0u(k))));
                #end
      
                #% increment k
                k=k+1 #k=k+1;
            case 0:
                #% unconstrained variable. x0u(i) is set.
                x0u[k] = x0[i] #x0u(k) = x0(i);
      
                #% increment k
                k=k+1 #k=k+1;
            case 4:
                #% fixed variable. drop it before fminsearch sees it.
                #% k is not incremented for this variable.
                k = k
        #end
    #end
    #% if any of the unknowns were fixed, then we need to shorten
    #% x0u now.
    
    if (k <= n) : #k<=n
        x0u[k:n] = [] #x0u(k:n) = [];
    #end
        
    output = []
    #% were all the variables fixed?
    if len(x0u)==0: #isempty(x0u)
        #% All variables were fixed. quit immediately, setting the
        #% appropriate parameters, then return.
  
        #% undo the variable transformations into the original space
        x = xtransform(x0u,params) #x = xtransform(x0u,params);
  
        #% final reshape
        x = np.reshape(x,xsize)
  
        #% stuff fval with the final value
        fval = params['fun'](x,params['args']) #fval = feval(params.fun,x,params.args{:});
  
        #% fminsearchbnd was not called
        exitflag = 0
  
        output['iterations'] = 0 #output.iterations = 0;
        output['funcCount'] = 1 #output.funcCount = 1;
        output['algorithm'] = 'fminsearch' #output.algorithm = 'fminsearch';
        output['message'] = 'All variables were held fixed by the applied bounds' #output.message = 'All variables were held fixed by the applied bounds';
  
        #% return with no call at all to fminsearch
        return([x,fval,exitflag,output])
    #end

    #% Check for an outputfcn. If there is any, then substitute my
    #% own wrapper function.
    if len(options['OutputFcn']): #~isempty(options.OutputFcn)
        params['OutputFcn'] = options['OutputFcn'] #params.OutputFcn = options.OutputFcn;
        options['OutputFcn'] = outfun_wrapper #options.OutputFcn = @outfun_wrapper;
    #end

    ##????????????????????????????????????????????    
    ######################################    
    #% now we can call fminsearch, but with our own
    #% intra-objective function.
    [xu,fval,exitflag,output] = fminsearch(intrafun,x0u,options,params) #[xu,fval,exitflag,output] = fminsearch(@intrafun,x0u,options,params);

    #% undo the variable transformations into the original space
    x = xtransform(xu,params)

    #% final reshape to make sure the result has the proper shape
    x = np.reshape(x,xsize) #x = reshape(x,xsize);

    #% Use a nested function as the OutputFcn wrapper
    def outfun_wrapper(x,varargin): #function stop = outfun_wrapper(x,varargin);
        #% we need to transform x first
        xtrans = xtransform(x,params) #xtrans = xtransform(x,params);
    
        #% then call the user supplied OutputFcn
        stop = params['OutputFcn'](xtrans,varargin[1:(len(varargin)-1)]) #stop = params.OutputFcn(xtrans,varargin{1:(end-1)});
        return(stop)
    #end

#end % mainline end
    




    
