class classic_nelder_mead_solver:
    def __init__(self,
                 initial_state,
                 degrees_of_freedom,
                 reflector = 1.0,
                 expander  = 2.0,
                 contractor= 0.5,
                 shrinker  = 0.5,
                 convergence_threshold = 1e-8,
                 verbose_level = 1):

        #Classic Nelder Mead Parameters
        #https://en.wikipedia.org/wiki/Nelder-Mead_method

        self.reflector      = reflector
        self.expander       = expander
        self.contractor     = contractor
        self.shrinker       = shrinker
        self.conv_threshold = convergence_threshold

        self.dofs  = degrees_of_freedom
        self.state = np.zeros((self.dofs+1,self.dofs+1))
        self.state[0:self.dofs,:] = initial_state
        self.status = "Initializing"

        self.sanity_check()
        self.verbose_level = verbose_level
        self.function_eval_count = 0
        self.initialize_triangle()
        self.sort_state()

    def sanity_check(self):
        state_shape = self.state.shape
        if(state_shape[1] != state_shape[0]):
            print("Your initial guess is malformed.")

    def initialize_triangle(self):
        for indx in range(self.dofs):
            position = self.state[0:self.dofs,indx]
            self.state[self.dofs,indx] = self.evaluate_objective_function(position)

    def sort_state(self):
        self.state = self.state[:,self.state[self.dofs,:].argsort()]

    def get_standing_best(self):
        self.standing_best = self.state[self.dofs,0]

    def evaluate_objective_function(self, position):
        #Objective Function
        #out = -1.0 * objective_function(position)
        self.function_eval_count += 1
        out = obj(position)
        return out

    def optimize(self):
        self.sort_state()
        self.get_standing_best()
        self.stop_flag = False
        current_best = 1.0 * self.standing_best
        self.iteration_count = 0
        while not self.stop_flag:
            self.iterate()
            std_dev = np.std(self.state[self.dofs,:])
            if(std_dev < self.conv_threshold):
                self.stop_flag = True
        if self.verbose_level > 0:
            print("Total Iterations: " + str(self.iteration_count))
            print("Total Function Evaluations: " + str(self.function_eval_count))

    def update_state(self,position,feval):
        self.state[self.dofs,-1] = feval
        self.state[0:self.dofs,-1] = position

    def update_by_shrinkage(self):
        best_state = self.state[0:self.dofs,0]
        for col in range(1,self.dofs+1):
            current_state    = self.state[0:self.dofs, col]
            contracted_state = best_state + self.contractor*(current_state - best_state)
            self.state[0:self.dofs,col]   = contracted_state
            self.state[self.dofs,  col]   = self.evaluate_objective_function(contracted_state)

    def iterate(self):
        #ORDER
        self.sort_state()
        self.iteration_count+=1

        remaining_points = self.state[0:self.dofs,0:self.dofs]
        #Compute the centroid of the facet opposite the worst point
        face_centroid    = np.einsum('ij->i',remaining_points) * (1.0 / self.dofs)

        #Compute and evaluate the reflection point
        reflection     = face_centroid + self.reflector*(face_centroid - self.state[0:self.dofs,-1])
        f_reflection     = self.evaluate_objective_function(reflection)

        f_best      = self.state[self.dofs,0]
        f_worst     = self.state[self.dofs,-1]
        f_2nd_worst = self.state[self.dofs,-2]

        if(self.verbose_level > 1):
            self.verbose_header()

        if((f_best <= f_reflection) and (f_reflection < f_2nd_worst)):
            self.update_state(reflection, f_reflection)
            self.status = "Reflection"
        elif(f_reflection < f_best):
            expansion   = face_centroid + self.expander*(reflection - face_centroid)
            f_expansion = self.evaluate_objective_function(expansion)
            if(f_expansion < f_reflection):
                self.update_state(expansion, f_expansion)
                self.status = "Expansion"
            else:
                self.update_state(reflection, f_reflection)
                self.status = "Reflection"
        else:
            contraction   = face_centroid + self.contractor * (self.state[0:self.dofs,-1] - face_centroid)
            f_contraction = self.evaluate_objective_function(contraction)
            if(f_contraction < f_worst):
                self.update_state(contraction, f_contraction)
                self.status = "Contraction"
            else:
                self.update_by_shrinkage()
                self.status = "Shrinkage"
        if self.verbose_level > 1:
            self.verbose_footer()

    def verbose_header(self):
        print("--------------------------------------------------------------------------------")
        print("Iteration " + str(self.iteration_count))
        print("Current Best: " + str(self.state[self.dofs,0]))

    def verbose_footer(self):
        print("Iteration Outcome: " + str(self.status))
        print("--------------------------------------------------------------------------------")