NMPC_controller.py

# Import standard libraries
from dataclasses import dataclass, field
from typing import Callable, Dict, List, Any, Union, Optional, Tuple
from dataclasses_json import dataclass_json
import numpy as np
import pandas as pd
import logging
import os
import sys
# Uncomment the following lines if CasADi is not installed in the main environment
# # Add to PATH the path where the casadi library was installed
# path_to_casadi_library = 'c:\\modelonimpact-1.8.1\\oct-dist\\myenv\\lib\\site-packages'  # Example path where CasADi 3.6.6 is installed
# if os.path.exists(path_to_casadi_library):
#     sys.path.insert(0, path_to_casadi_library)
import casadi as ca
# Import custom utilities and functions
from model_input import*
from general_utils.filter_df_by_timestamp import*
from general_utils.compare_arrays import*
from general_utils.read_file import*

# Define an external cost function for the nominal problem of the NMPC controller
def cost_function_nominal(Uk: ca.DM, Xk: ca.DM, dUk: ca.DM,
                            weights_y: np.ndarray, weights_du: float,
                            one_step: Callable, g: Callable,
                            Xk_scales: np.ndarray, Yk_scales: np.ndarray, uk_scale: float,
                            y_ref: pd.DataFrame, params: Dict[str, Tuple[float, float]],
                            nintervals: int, current_timestamp: pd.Timestamp, end_timestamp: pd.Timestamp, MPC_interval: int,
                            u_values_discrete: np.ndarray, param_u_values_discrete: np.ndarray, delta_t: np.ndarray, timestamp_points_discrete: np.ndarray,
                            mesh_finesse: int = 12, **kwargs) -> Tuple[ca.DM, Dict[str, float], List[pd.Timestamp], List[ca.MX], List[ca.MX], List[ca.MX], List[ca.MX]]:
    '''
    Formulate the cost function for the nominal NMPC problem.
    CasADi formulation to allow automatic differentiation from symbolic expressions.
    Args:
        Uk (ca.DM): Control input trajectory over the prediction horizon.
        Xk (ca.DM): State trajectory over the prediction horizon.
        dUk (ca.DM): Control input rate of change over the prediction horizon.
        weights_y (np.ndarray): Weights for the output tracking error in the cost function.
        weights_du (float): Weight for the control input rate of change in the cost function.
        one_step (Callable): Function to perform one step of system integration.
        g (Callable): Output function mapping states to outputs.
        Xk_scales (np.ndarray): Scaling factors for the states.
        Yk_scales (np.ndarray): Scaling factors for the outputs.
        uk_scale (float): Scaling factor for the controllable inputs.
        y_ref (pd.DataFrame): Reference output trajectory for tracking (setpoints!!).
        params (Dict[str, Tuple[float, float]]): Dictionary of model parameters and their scale.
        nintervals (int): Number of control intervals in the prediction horizon.
        current_timestamp (pd.Timestamp): Current timestamp at the start of the prediction horizon.
        end_timestamp (pd.Timestamp): End timestamp at the end of the prediction horizon.
        MPC_interval (int): Control interval duration in hours.
        u_values_discrete (np.ndarray): Discrete flow-rate input values for system integration.
        param_u_values_discrete (np.ndarray): Discrete influent characterization parameter values for system integration.
        delta_t (np.ndarray): Time step sizes for system integration.
        timestamp_points_discrete (np.ndarray): Discrete timestamp points for system integration.
        mesh_finesse (int, optional): Number of integration steps per hour. Defaults to 12 (5 minutes per step).
    Returns:
        Tuple[ca.DM, Dict[str, float], List[pd.Timestamp], List[ca.MX], List[ca.MX], List[ca.MX], List[ca.MX]]: 
        A tuple containing the cost function value (scalar), a dictionary of additional information, a list of result timestamps,
        and lists of state and output trajectories.

    Note: must have consistency between 'nintervals', 'current_timestamp', 'end_timestamp' and 'timestamp_points_discrete'!!
    Note: the last two outputs (z_star_list, y_star_list) are empty lists in this nominal formulation. 
        Needed anyway to have a common interface with the ancillary formulation, inside the NMPC class ('set_external_cost_function' method).
    Note: kwargs can contain additional parameters for slackness (if used).
    Note: in this version, there are 'tracking_cost' (simulation error with 'y_ref' setpoints) and 'controlrate_cost' as separate terms in the cost function.
        No panalization on input utilization is included here.
    Note: in this version, the function extracts output in prediction at every hour allowing for more flexibility for the next control step, i.e. asynchronous NMPC.
    '''
    # Extract additional parameters and variables from kwargs
    # Presence of slack variables and related weights
    slackness = kwargs.get('slackness', False)
    if slackness:
        eps1 = kwargs.get('eps1')
        eps2 = kwargs.get('eps2')
        weights_eps1 = kwargs.get('weights_eps1')
        weights_eps2 = kwargs.get('weights_eps2')
    #----------------------------------------------------------------------------------------------------------- #    
    # Declare cost function
    tracking_cost = 0 # Initialize tracking cost of the 'y_ref' setpoints
    x_list = []
    y_list = [g(Xk[:,0], ca.DM([value[0] for value in params.values()]))]
    result_timestamps = [current_timestamp]
    time_index_at_k = 0
    for k in range(nintervals):
        nsteps_in_interval = timestamp_points_discrete[(timestamp_points_discrete >= current_timestamp) & (timestamp_points_discrete<= current_timestamp + pd.Timedelta(hours=MPC_interval))][:-1].shape[0]
        if nsteps_in_interval != mesh_finesse*MPC_interval:
            logging.warning(f'Steps in interval are {nsteps_in_interval}, but they must be {mesh_finesse*MPC_interval}.')
        #----------------------------------------------------------------------------------------------------------- #   
        uk = Uk[k]
        # Integrate over the interval
        x = Xk[:,k]
        # time_index_at_k = k*nsteps_in_interval # Note: uncomment if does not start from 0 (not in online application but maybe in simulation testing)
        time_index_at_kplus1 = time_index_at_k + nsteps_in_interval
        # Call openloop model to simulate over the interval
        for i in range(time_index_at_k, time_index_at_kplus1, 1):
            x = one_step(x,
                        ca.vertcat(ca.DM(list(u_values_discrete[i,:-1])),uk),
                        param_u_values_discrete[i].T,
                        ca.DM(delta_t[i]),
                        ca.DM([value[0] for value in params.values()]) # In prediction mode, the parameters are fixed.
                        )
            y = g(x,
                  ca.DM([value[0] for value in params.values()]))
            
            # Note: append values at each hour (added on 25.05.2025 to allow sporadic NMPC runs)
            if (i - time_index_at_k) != 0 and (i - time_index_at_k) % mesh_finesse == 0:
                x_list.append(x)
                y_list.append(y)

        x_list.append(x) # Note: added for Multiple-Shooting formulation
        y_list.append(y)
        #----------------------------------------------------------------------------------------------------------- # 
        # Update timestamps and indexes
        current_timestamp_otherway = current_timestamp + pd.Timedelta(hours=MPC_interval) 
        current_timestamp = timestamp_points_discrete[time_index_at_kplus1]
        if current_timestamp_otherway != current_timestamp:
            logging.warning(f'Current timestamp adding control interval is {current_timestamp_otherway}, but adding time_indexes is {current_timestamp}')
        result_timestamps.append(current_timestamp)
        time_index_at_k += nsteps_in_interval
        #----------------------------------------------------------------------------------------------------------- # 
        # Compute tracking cost at the end of the interval (i.e. at each control step)
        if y_ref[y_ref['Timestamp'] == current_timestamp].empty:
            logging.warning(f'Reference data at {current_timestamp} is missing.')
        track_error = ca.vertcat(
            weights_y[0]*((y[0]*y[1] - y_ref[(y_ref['Timestamp']==current_timestamp)].values[0,1])/Yk_scales[0])**2,
            weights_y[1]*((y[2]/y[1] - y_ref[(y_ref['Timestamp']==current_timestamp)].values[0,2])/Yk_scales[1])**2)
        if weights_y.shape[0] > 2: # Note: add vertically if S2_ref is present in y_ref
            track_error = ca.vertcat(track_error, weights_y[2]*((x[6] - y_ref[(y_ref['Timestamp']==current_timestamp)].values[0,3])/Yk_scales[2])**2)
        # tracking_cost += track_error.T@track_error  # Add quadratic cost on control
        tracking_cost += ca.sum1(track_error)  # Add quadratic cost on control
        #----------------------------------------------------------------------------------------------------------- # 
    logging.info(f'MPC predicts from {result_timestamps[0]} to {current_timestamp}')
    # Final check of timestamps
    if current_timestamp != end_timestamp:
        logging.warning(f'{current_timestamp} is different from {end_timestamp}.')
    # Final objective (sum of costs)
    dUk_scaled = dUk*ca.repmat(uk_scale,1,nintervals-1)
    controlrate_cost = dUk_scaled@dUk_scaled.T
    cost = weights_du*controlrate_cost + tracking_cost
    costs_dict = {'tracking_cost': tracking_cost, 'controlrate_cost': controlrate_cost}
    # Add slackness cost if applicable
    if slackness:
        slack_ub_cost = ca.sum1((eps2/Xk_scales)**2)
        slack_lb_cost = ca.sum1((eps1/Xk_scales)**2)
        cost += weights_eps1*slack_lb_cost + weights_eps2*slack_ub_cost
        costs_dict.update({'slack_ub_cost': slack_ub_cost, 'slack_lb_cost': slack_lb_cost})
    # Return empty lists for z_star and y_star in nominal formulation (see note in docstring)
    z_star_list = []
    y_star_list = []
    return cost, costs_dict, result_timestamps, x_list, y_list, z_star_list, y_star_list

# Define an external cost function for the ancillary problem of the NMPC controller
def cost_function_ancillary(Uk: ca.DM, Xk: ca.DM, dUk: ca.DM,
                            weights_y: np.ndarray, weights_du: float,
                            one_step: Callable, g: Callable,
                            Xk_scales: np.ndarray, Yk_scales: np.ndarray, uk_scale: float,
                            y_ref: pd.DataFrame, params: Dict[str, Tuple[float, float]],
                            nintervals: int, current_timestamp: pd.Timestamp, end_timestamp: pd.Timestamp, MPC_interval: int,
                            u_values_discrete: np.ndarray, param_u_values_discrete: np.ndarray, delta_t: np.ndarray, timestamp_points_discrete: np.ndarray,
                            mesh_finesse: int = 12, **kwargs):
    '''
    Formulate the cost function for the ancillary NMPC problem.
    CasADi formulation to allow automatic differentiation from symbolic expressions.
    Args:
        Uk (ca.DM): Control input trajectory over the prediction horizon.
        Xk (ca.DM): State trajectory over the prediction horizon.
        dUk (ca.DM): Control input rate of change over the prediction horizon.
        weights_y (np.ndarray): Weights for the output tracking error in the cost function.
        weights_du (float): Weight for the control input rate of change in the cost function.
        one_step (Callable): Function to perform one step of system integration.
        g (Callable): Output function mapping states to outputs.
        Xk_scales (np.ndarray): Scaling factors for the states.
        Yk_scales (np.ndarray): Scaling factors for the outputs.
        uk_scale (float): Scaling factor for the controllable inputs.
        y_ref (pd.DataFrame): Reference output trajectory for tracking (setpoints!!).
        params (Dict[str, Tuple[float, float]]): Dictionary of model parameters and their scale.
        nintervals (int): Number of control intervals in the prediction horizon.
        current_timestamp (pd.Timestamp): Current timestamp at the start of the prediction horizon.
        end_timestamp (pd.Timestamp): End timestamp at the end of the prediction horizon.
        MPC_interval (int): Control interval duration in hours.
        u_values_discrete (np.ndarray): Discrete flow-rate input values for system integration.
        param_u_values_discrete (np.ndarray): Discrete influent characterization parameter values for system integration.
        delta_t (np.ndarray): Time step sizes for system integration.
        timestamp_points_discrete (np.ndarray): Discrete timestamp points for system integration.
        mesh_finesse (int, optional): Number of integration steps per hour. Defaults to 12 (5 minutes per step).
    Returns:
        Tuple[ca.DM, Dict[str, float], List[pd.Timestamp], List[ca.MX], List[ca.MX], List[ca.MX], List[ca.MX]]: 
        A tuple containing the cost function value (scalar), a dictionary of additional information, a list of result timestamps,
        and lists of state and output trajectories, as well as optimal state and output trajectories from the nominal problem.

    Note: must have consistency between 'nintervals', 'current_timestamp', 'end_timestamp' and 'timestamp_points_discrete'!!
    Note: kwargs can contain additional the additional meta-parameters and reference trajectories needed for the ancillary formulation:
        - formulation (str): 'ancillaryoffline' or 'ancillaryonline'.
        - z_star (ca.DM): Optimal state trajectory from nominal problem (for ancillaryoffline).
        - z_star0 (ca.DM): Initial state decision variable (for ancillaryonline).
        - weights_xstar (float): Weight for state tracking error from nominal trajectory.
        - weights_ustar (float): Weight for input tracking error from nominal trajectory.
        - weights_yN (float): Weight for terminal setpoints' tracking error.
        - v_star (ca.DM): Optimal control trajectory from nominal problem.
    Note: in this version, there are 'tracking_cost' (simulation error with 'y_ref' setpoints) and 'controlrate_cost' as separate terms in the cost function.
        No panalization on input utilization is included here. 
        However, there is also a tracking cost on the deviation from the nominal optimal trajectories (states and inputs i.e. z_star and v_star).
    Note: in this version, the function extracts output in prediction at every hour allowing for more flexibility for the next control step, i.e. asynchronous NMPC.
    '''
    # Extract additional parameters, decision variables and/or reference trajectories from kwargs
    formulation = kwargs.get('formulation', 'ancillaryoffline')
    if formulation == 'ancillaryonline': # Note: if tube-online, get z_star0 from decision variables
        z_star0 = kwargs.get('z_star0', None)
        z_stark = z_star0
    if formulation != 'ancillaryonline': # Note: if tube-offline, define the initial condition of the state decision variables equal to the one given as input (z_star)
        z_star = kwargs.get('z_star')
    weights_xstar = kwargs.get('weights_xstar')
    weights_ustar = kwargs.get('weights_ustar')
    weights_yN = kwargs.get('weights_yN')
    v_star = kwargs.get('v_star') # Note: optimal control trajectory from the nominal problem to be used in the ancillary problem
    #----------------------------------------------------------------------------------------------------------- #     
    # Declare cost function
    tracking_cost_x = 0
    tracking_cost_u = 0
    x_list = []
    y_list = [g(Xk[:,0], ca.DM([value[0] for value in params.values()]))]
    z_star_list = []
    y_star_list = []
    if formulation == 'ancillaryonline': # Note: if tube-online, get z_star0 from decision variables
        z_star_list = [z_star0]
        y_star_list = [g(z_star0,ca.DM([value[0] for value in params.values()]))]
    result_timestamps = [current_timestamp]
    time_index_at_k = 0
    for k in range(nintervals):
        nsteps_in_interval = timestamp_points_discrete[(timestamp_points_discrete >= current_timestamp) & (timestamp_points_discrete<= current_timestamp + pd.Timedelta(hours=MPC_interval))][:-1].shape[0]
        if nsteps_in_interval != mesh_finesse*MPC_interval:
            logging.warning(f'Steps in interval are {nsteps_in_interval}, but they must be {mesh_finesse*MPC_interval}.')
        vk = v_star[:,k]
        if formulation != 'ancillaryonline': # Note: if tube-offline, define the initial condition of the state decision variables equal to the one given as input (z_star)
            z_stark = z_star[:,k]
        #----------------------------------------------------------------------------------------------------------- #    
        uk = Uk[k]
        # Integrate over the interval
        x = Xk[:,k]
        #----------------------------------------------------------------------------------------------------------- #    
        track_error_x = (x - z_stark)/Xk_scales
        track_error_u = (uk - vk)*uk_scale

        tracking_cost_x += track_error_x.T@track_error_x  
        tracking_cost_u += track_error_u.T@track_error_u #Does it work like this?
        #----------------------------------------------------------------------------------------------------------- #    
        # Call openloop model to optimize zstar(ti)
        time_index_at_kplus1 = time_index_at_k + nsteps_in_interval
        if formulation == 'ancillaryonline': # Note: if tube-online, call twice the openloop model. Here starting from z_star0 and apply control actions vk as given from function input
            for i in range(time_index_at_k, time_index_at_kplus1, 1):
                z_stark = one_step(z_stark,
                            ca.vertcat(ca.DM(list(u_values_discrete[i,:-1])),vk),
                            param_u_values_discrete[i].T,
                            ca.DM(delta_t[i]),
                            ca.DM([value[0] for value in params.values()])
                            )
                y_star = g(z_stark,ca.DM([value[0] for value in params.values()]))

                # Note: append values at each hour (added on 25.05.2025 to allow sporadic NMPC runs)
                if (i - time_index_at_k) != 0 and (i - time_index_at_k) % mesh_finesse == 0:
                    z_star_list.append(z_stark)
                    y_star_list.append(y_star)

            z_star_list.append(z_stark)
            y_star_list.append(y_star)
        #----------------------------------------------------------------------------------------------------------- #
        # Call openloop model to simulate over the interval     
        for i in range(time_index_at_k, time_index_at_kplus1, 1):
            x = one_step(x,
                        ca.vertcat(ca.DM(list(u_values_discrete[i,:-1])),uk),
                        param_u_values_discrete[i].T,
                        ca.DM(delta_t[i]),
                        ca.DM([value[0] for value in params.values()])
                        )
            y = g(x,ca.DM([value[0] for value in params.values()]))

            # Note: append values at each hour (added on 25.05.2025 to allow sporadic NMPC runs)
            if (i - time_index_at_k) != 0 and (i - time_index_at_k) % mesh_finesse == 0:
                x_list.append(x)
                y_list.append(y)

        x_list.append(x) # Note: added for Multiple-Shooting formulation
        y_list.append(y)
        #----------------------------------------------------------------------------------------------------------- #     
        # Update timestamps and indexes
        current_timestamp_otherway = current_timestamp + pd.Timedelta(hours=MPC_interval)
        current_timestamp = timestamp_points_discrete[time_index_at_kplus1]
        if current_timestamp_otherway != current_timestamp:
            logging.warning(f'Current timestamp adding control interval is {current_timestamp_otherway}, but adding time_indexes is {current_timestamp}')
        result_timestamps.append(current_timestamp)
        time_index_at_k += nsteps_in_interval
    #----------------------------------------------------------------------------------------------------------- #
    # Compute tracking cost at the end of the interval (i.e. at each control step)
    logging.info(f'MPC predicts from {result_timestamps[0]} to {current_timestamp}')
    # Final check of timestamps
    if current_timestamp != end_timestamp:
        logging.warning(f'{current_timestamp} is different from {timestamp_points_discrete[-1]}.')
    # Final objective (sum of costs)
    dUk_scaled = dUk*ca.repmat(uk_scale,1,nintervals-1)
    controlrate_cost = dUk_scaled@dUk_scaled.T
    terminal_error = ca.vertcat(
            weights_y[0]*((y[0]*y[1] - y_ref[(y_ref['Timestamp']==current_timestamp)].values[0,1])/Yk_scales[0])**2,
            weights_y[1]*((y[2]/y[1] - y_ref[(y_ref['Timestamp']==current_timestamp)].values[0,2])/Yk_scales[1])**2)
    if weights_y.shape[0] > 2: # Note: add vertically if S2_ref is present in y_ref
        terminal_error = ca.vertcat(terminal_error, weights_y[2]*((x[6] - y_ref[(y_ref['Timestamp']==current_timestamp)].values[0,3])/Yk_scales[2])**2)
    terminal_cost = ca.sum1(terminal_error)
    cost = weights_du*controlrate_cost + weights_xstar*tracking_cost_x + weights_ustar*tracking_cost_u + weights_yN*terminal_cost
    costs_dict = {'terminal_cost': terminal_cost, 'controlrate_cost': controlrate_cost,
                  'tracking_cost_x': tracking_cost_x, 'tracking_cost_u': tracking_cost_u}
    return cost, costs_dict, result_timestamps, x_list, y_list, z_star_list, y_star_list

############################################################################################################
@dataclass_json
@dataclass
class NMPC:
    """
    A class for the NMPC controller block, which, based on the predictions of the 'model' object initialized in the 'init_state', 
    computes the optimal control action solving a constrained optimization problem, satisfying the 'constraints' and minimizing the 'cost_function'.
    Designed for integration with open-loop model simulation, EKF and parameter online recursive update.
    Attributes:
        model (object): The model object containing the system dynamics and outputs as CasADI functions.
        modelname (str): Name of the model, for file naming purposes.
        var_couples_list (List): List of model-data variable couples (used to create the 'status' dictionary only).
        control_interval (float): Control interval in hours.
        N (np.ndarray): Prediction horizon in days.
        Nu (np.ndarray): Control horizon in days (must be less than or equal to N).
        ref_df (pd.DataFrame): DataFrame containing the reference trajectories with a 'Timestamp' column (setpoints!!).
    Optional attributes:
        integrator_parameters (Optional[Dict], optional): Dictionary containing integrator parameters (over which to evaluate the prediction model in the future). Defaults to None.
        u_values (np.ndarray, optional): Array of flow-rate input values for the prediction model over the simulation horizon. Defaults to None.
        param_u_values (np.ndarray, optional): Array of parameter characterization input values for the prediction model over the simulation horizon. Defaults to None.
        delta_t (np.ndarray, optional): Array of time step sizes for the prediction model over the simulation horizon. Defaults to None.
        time_points (np.ndarray, optional): Array of time points for the prediction model over the simulation horizon. Defaults to None.
        timestamp_points (np.ndarray, optional): Array of timestamp points for the prediction model over the simulation horizon. Defaults to None.
        state_scales (np.ndarray, optional): Scaling factors for the states. Defaults to None. Can be also object of type ca.DM.
        output_scales (np.ndarray, optional): Scaling factors for the outputs. Defaults to None. Can be also object of type ca.DM.
        input_scale (np.ndarray, optional): Scaling factors for the controllable inputs. Defaults to None. Can be also object of type ca.DM.
        cost_function (Callable, optional): Cost function to be minimized. Defaults to cost_function_nominal.
        init_state (Optional[np.ndarray], optional): Initial state of the system. Defaults to None.
        slackness (Optional[bool], optional): Whether to include slack variables in the optimization problem. Defaults to False.
        problemtype (str, optional): Type of NMPC problem ('NOMINAL' or 'ANCILLARY'). Defaults to 'NOMINAL'.
        formulation (str, optional): Formulation of the NMPC problem ('nominal', 'ancillaryoffline', 'ancillaryonline'). Defaults to 'nominal'.
        current_date (datetime, optional): Current date and time for file naming purposes. Defaults to datetime.now().replace(microsecond=0).
        opti_parameters (Optional[Dict[str, float]], optional): Dictionary of the controller meta-parameters for design/tuning (e.g. weights for the cost function). Defaults to None.
        costs_dict (Optional[Dict[str, float]], optional): Dictionary to store the different cost terms after optimization. Defaults to None.
        constraints_dict (Optional[Dict[str, float]], optional): Dictionary to store the different constraint after optimization. Defaults to None.
        opti (ca.Opti): CasADi Opti instance for the NMPC optimization problem.
        solution (ca.OptiSol): Solution of the NMPC optimization problem after solving. Defaults to None.
        x_list (ca.MX): List of symbolic state values in the multiple-shooting formulation (used to transfer values from cost function to constraints). Defaults to None.
        y_list (ca.MX): List of symbolic output values in the multiple-shooting formulation (used to extract y_star output values from opti.solution, for saving purposes). Defaults to None.
        logger (logging.Logger): Logger for logging information and warnings. Defaults to logging.getLogger(__name__).
        status (Optional[dict], optional): Dictionary to store the current status of the NMPC controller (e.g., current init_state, decision variables, etc..). Defaults to None.
        z_star (Optional[ca.DM], optional): Optimal state trajectory from the nominal problem to be used in the ancillary problem. Defaults to None.
        v_star (Optional[ca.DM], optional): Optimal control trajectory from the nominal problem to be used in the ancillary problem. Defaults to None.
        z_star_list (Optional[List[ca.MX]], optional): List of updated optimal state trajectories in the ancillaryonline formulation. Defaults to None.
        y_star_list (Optional[List[ca.MX]], optional): List of updated optimal output trajectories (consistent with z_star_list) in the ancillaryonline formulation. Defaults to None.
        
    Methods:
        __post_init__: Initializes the CasADi Opti instance and problem structure.
        add_constraints: Adds constraints to the Opti instance ("standard" constraints, internally defined).
        additional_ext_contraint: Adds additional external constraints to the Opti instance (user-defined externally).
        setup_solver: Sets up the solver for the Opti instance.
        set_external_cost_function: Sets the external 'cost_function' for the Opti instance (user-defined externally).
        set_cost_function: Sets the cost function for the Opti instance (internally defined inside the method itself; for the 'nominal' problem only).
        set_init_decvars: Sets the initial guess for the decision variables of the NMPC problem.
        set_init_state: Sets the initial state for the NMPC problem/'opti' instance (symbolic constraint on the initial states' decision variables).
        set_parameters: Sets the meta-parameters for the NMPC problem/'opti' instance (e.g. weights, bounds, etc..).
        run_optimization: Solves the NMPC optimization problem, extracts the solutions and updates the solution attributes (e.g. 'status')
        one_glance_opti_parameters: Prints a summary of the current NMPC opti_parameters for quick reference.
        prepare_inputs: Prepare model inputs before simulation, including influent characterization and flow rates.

    Note: init_state should be set, after instantiation, before calling the 'solve' method, calling the 'set_init_state' method. 
    Note: CasADi-OptiStack is used.
    Note: multiple-shooting formulation (states are considered as decision variables and system dynamics are imposed as constraints).
    Note: 'model' must contain CasADI-based functions for the model dynamics and outputs.
    Note: this version extracts output in prediction at every hour allowing for more flexibility for the next control step i.e. asynchronous NMPC.
    Note: run 'prepare_inputs', that reads the start/end timestamps from the 'integrator_parameters' attribute,
        to set 'start_timestamp'-'end_timestamp', as well as the other inputs required to perform integration
        ('u_values_discrete', 'param_u_values_discrete', 'delta_step_discrete', 'time_points_discrete')!!. 
    Note: and/or give specific inputs to the other internal methods to ovverride (once again/more).
    
    Note: different NMPC problem formulations are possible (see [Carecci *et al.*, 2026](https://doi.org/10.48550/arXiv.2601.01157) for more details and descriptions):
        formulation: str = 'nominal' or 'ancillaryoffline' or 'ancillaryonline'. 
            Regardless these parameters, two different NMPC classes must be instantiated externally for online tube-based NMPC version. 
        problemtype: str 'NOMINAL' or 'ANCILLARY' to create 'NOMINAL' and 'ANCILLARY' problems.
        In 'classical':     'fomulation' is always 'nominal'.
        In 'tube-offline':  The NOMINAL problem can have 'slackness' or not, and the 'formulation' is 'nominal'. 
                            The ANCILLARY problem has 'slackness'=False and the 'formulation' is 'ancillaryoffline'.
        In 'tube-online':   The NOMINAL problem can have 'slackness' or not, and the 'formulation' is 'ancillaryoffline'....why??? 'nominal'!!!!
                            The ANCILLARY problem has 'slackness'=False and the 'formulation' is 'ancillaryonline'.

    Recap of decision variables and parameters (and relation with official nomenclature of [Carecci *et al.*, 2026](https://doi.org/10.48550/arXiv.2601.01157)):
        Decision Variables:
            Xk: states over the prediction horizon (x in ancillary problem, z in nominal problem)
            Uk: control inputs over the prediction horizon (u in ancillary problem, v in nominal problem)
            dUk: control input rate of change over the prediction horizon (du in ancillary problem, dv in nominal problem)
            eps1, eps2: slack variables for state constraints (\epsilon_lb and \epsilon_ub only nominal problem)
            z_star0: initial value of the nominal states re-optimized in the ancillary problem (only ancillary problem, online version)
        Other quantities:
            z_star: optimal nominal states over the prediction horizon (only ancillary problem, offline version)
            v_star: optimal nominal control inputs over the prediction horizon (only ancillary problem)
            x_star, u_star: name given to the optimal values of Xk and Uk
            y_star: name given to optimal outputs over the prediction horizon (evaluate from x_star)
        Parameters:
            weights_y: weights for output tracking error in the cost function (W_y in paper)
            weights_du: weight for control input rate of change in the cost function (not present in paper, added for practical reasons)
            weights_xstar: weight for nominal states tracking error in the ancillary cost function (only ancillary problem) (w_x in paper)
            weights_ustar: weight for nominal control inputs tracking error in the ancillary cost function (only ancillary problem) (w_u in paper)
            weights_yN: weight for terminal setpoints' tracking error in the ancillary cost function (only ancillary problem) (w_{y_{H_p}} in paper)
            bnds_x: bounds on the states (bounds on Xk, X in ancillary problem, Z in nominal problem)
            bnds_u: bounds on the control inputs (bounds on Uk, U in ancillary problem, same in nominal problem)
            bnds_du: bounds on the control input rate of change (bounds on dUk, DU in ancillary problem, same in nominal problem)
            bnds_z: bounds on the nominal states in the ancillaryonline formulation (only ancillary problem, online version) 
                (bounds on optimal state trajectory computed starting from z_star0, Z in paper)

    Note: 'x_star' is extracted and saved extenally, instead of 'x_list',
        since it considers 'init_state' and not the point computed after integration on the last interval. 
        Actually, to warm-up, what is done is to remove the initial point from 'x_star' and duplicate the last one 
        (in future it could also be possible to use directly 'x_list' to warm-up the states...but not possible with the control actions).
    IMPORTANT NOTE: it is assumed in the decision variables' definition inside '__post_init__' that the 
                    control action is only one-dimensional (single input system)!!! User should modify accordingly if needed.
    """
    model: object
    modelname: str
    var_couples_list: List                                
    control_interval: float
    N: np.ndarray
    Nu: np.ndarray
    ref_df: pd.DataFrame
    
    integrator_parameters: Optional[Dict] = None
    u_values: np.ndarray = field(init=True, default=None)
    param_u_values: np.ndarray = field(init=True, default=None)
    delta_t: np.ndarray = field(init=True, default=None)
    time_points: np.ndarray = field(init=True, default=None)
    timestamp_points: np.ndarray = field(init=True, default=None)

    state_scales: np.ndarray= None  
    output_scales: np.ndarray = None
    input_scale: np.ndarray = None

    cost_function: Callable = cost_function_nominal                 # Note: used in new implementation (07.02.2025)
    init_state: Optional[np.ndarray] = None
    slackness: Optional[bool] = False
    problemtype: str = 'NOMINAL' 
    formulation: str = 'nominal'
    current_date: datetime = datetime.now().replace(microsecond=0)  # Note: ROUND?? .round('T') to round to the nearest minute, but only with pd.Timestamp

    opti_parameters: Optional[Dict[str, float]] = None              
    costs_dict: Optional[Dict[str, float]] = None
    constraints_dict: Optional[Dict[str, float]] = None
    opti: Optional[ca.Opti] = field(init=False)
    solution: Optional[ca.OptiSol] = None
    x_list: Optional[List[ca.MX]] = None
    y_list: Optional[List[ca.MX]] = None
    logger: logging.Logger = logging.getLogger(__name__)
    status: Optional[dict] = None

    z_star: Optional[ca.DM] = None                                  # Note: used only in the ANCILLARY cases
    v_star: Optional[ca.DM] = None                                  # Note: used only in the ANCILLARY cases 
    z_star_list: Optional[List[ca.MX]] = None                       # Note: used only in the ANCILLARY cases 
    y_star_list: Optional[List[ca.MX]] = None                       # Note: used only in the ANCILLARY cases 

    def __post_init__(self):
        """
        Form the NMPC optimization problem using CasADi-OptiStack.
        Initialize the CasADi Opti instance and problem structure.
        Declare the OptiStack decision variables and parameters.
        Decision Variables and Parameters declared:
            all problems and formulations: 
                VAR: {Uk, Xk}; PARAM: {weights_y, weights_du, bnds_u, bnds_du, bnds_x};
            if slackness: 
                VAR: {eps1, eps2}; PARAM: {weights_eps1, weights_eps2};
            if problem = ancillary: 
                PARAM: {weights_xstar, weights_ustar, weights_yN};
            if problem = ancillary and problem = ancillaryonline: 
                VAR: {z_star0}, PARAM: {weights_xstar, weights_ustar, weights_yN, bnds_z}.
        """
        self.logger.info('-----------------------------------------------\n')
        self.logger.info(f'Instantiting Opti. CasADi version loaded is {ca.__version__}')
        self.opti = ca.Opti()

        # DEFINE SOME META-QUANTITIES
        self.nintervals = int(self.N*(24/self.control_interval))                                                # Number of intervals over which to solve the optimization problem
        self.logger.info(f'Number of intervals within the prediction horizon is {self.nintervals}')
        self.nintervals_moveblocking = int(self.Nu*(24/self.control_interval))                                  # Number of intervals over which to solve the optimization problem with move-blocking
        if self.state_scales is None or self.output_scales is None or self.input_scale is None:
            self.state_scales = ca.DM.ones(self.model.init_state.shape[0])
            self.output_scales = ca.DM.ones(self.ref_df.shape[1]-1)                                             # Not intended anymore as [qTTot, xM_gb, xC_gb], but as the 'y_ref' present in the 'ref_df' and in 'cost_function'. No time-varying!!
            self.input_scale = ca.DM.ones(1)
            self.logger.info('Some scales are not given as input, set to default 1.')
        self.state_scales = ca.DM(self.state_scales)
        self.output_scales = ca.DM(self.output_scales)
        self.input_scale = ca.DM(self.input_scale)
        
        # CHECK COHERENCY BETWEEN 'COST_FUNCTION', 'SLACKNESS' AND 'FORMULATION'
        if self.problemtype == 'NOMINAL':
            self.cost_function = cost_function_nominal
        else:
            self.cost_function = cost_function_ancillary
        if self.problemtype == 'NOMINAL' and self.formulation == 'ancillaryonline':
            self.logger.error('Incoherent choice of problemtype and formulation.')
            return ValueError('Incoherent choice of problemtype and formulation.')
        elif self.problemtype == 'ANCILLARY' and self.formulation == 'nominal' or self.problemtype == 'ANCILLARY' and self.slackness:
            self.logger.error('Incoherent choice of problemtype and formulation or problemtype and slackness.')
            return ValueError('Incoherent choice of problemtype and formulation or problemtype and slackness.')
        
        # DECLARE OPTIMIZATION VARIABLES
        self.Uk = self.opti.variable(1,self.nintervals)/ca.DM(ca.repmat(self.input_scale,1,self.nintervals))    # Note: assumes single input for the moment!!!
        self.Xk = self.opti.variable(self.model.init_state.shape[0],self.nintervals)*ca.repmat(self.state_scales,1,self.nintervals)
        self.dUk = self.Uk[:,1:] - self.Uk[:,:-1]
        if self.slackness:
            self.eps1 = self.opti.variable(self.model.init_state.shape[0],1)*self.state_scales                  # Slack on states (that are decision variables) -> LOWER BOUNDS
            self.eps2 = self.opti.variable(self.model.init_state.shape[0],1)*self.state_scales                  # Slack on states (that are decision variables) -> UPPER BOUNDS
        if self.formulation == 'ancillaryonline':
            self.z_star0 = self.opti.variable(self.model.init_state.shape[0],1)*self.state_scales
        
        # DECLARE OPTI PARAMETERS
        self.weights_y = self.opti.parameter(self.ref_df.shape[1]-1,1)
        self.weights_du = self.opti.parameter()
        self.bnds_u = self.opti.parameter(1,2)                                                                  # Note: assumes single input for the moment!!!
        self.bnds_du = self.opti.parameter(1,2)                                                                 # Note: assumes single input for the moment!!!
        self.bnds_x = self.opti.parameter(self.model.init_state.shape[0],2)
        self.opti_parameters = {'weights_y': self.weights_y, 'weights_du': self.weights_du, 'bnds_u': self.bnds_u, 'bnds_du': self.bnds_du, 'bnds_x': self.bnds_x}
        if self.slackness:
            self.weights_eps1 = self.opti.parameter()
            self.weights_eps2 = self.opti.parameter()
            self.opti_parameters.update({'weights_eps1': self.weights_eps1, 'weights_eps2': self.weights_eps2})
        if self.formulation != 'nominal':
            self.weights_xstar = self.opti.parameter()                                                          # Note: better to specify it as a square diagonal matrix to give a weight to each state trajectory?
            self.weights_ustar = self.opti.parameter()                                                          # Note: better to specify it as a square diagonal matrix to give a weight to each control trajectory?
            self.weights_yN = self.opti.parameter()
            self.opti_parameters.update({'weights_xstar': self.weights_xstar, 'weights_ustar': self.weights_ustar, 'weights_yN': self.weights_yN})
        if self.formulation == 'ancillaryonline':
            self.bnds_z = self.opti.parameter(self.model.init_state.shape[0],2)
            self.opti_parameters.update({'bnds_z': self.bnds_z})

        # DEFINE STATUS DICTIONARY
        # Note: uncommented entries can be added if needed
        status = {
            'Timestamp': self.integrator_parameters['start_timestamp'],
            'end': self.integrator_parameters['end_timestamp'],
            'run_Timestamp': self.current_date,
            # 'Control_interval': self.control_interval,
            # 'Prediction_horizon': self.N,
            # 'Control_horizon': self.Nu,
        }
        self.status = status
    ############################################################################################################

    def add_constraints(self, constraints: Optional[List[Callable]] = None) -> None:
        """Function to add constraints to the Opti instance."""
        self.logger.info('## Adding constraints to the Opti instance...##')
        # Define a dictionary to store constraints for logging purposes
        self.constraints_dict = {}
        
        # INPUT BOX BOUNDS
        bnd_u = self.opti.bounded(self.bnds_u[:,0], self.Uk, self.bnds_u[:,1])
        self.opti.subject_to(bnd_u)
        
        # INPUT RATE BOX BOUNDS
        bnd_du = self.opti.bounded(self.bnds_du[:,0], self.dUk, self.bnds_du[:,1])
        self.opti.subject_to(bnd_du)
        
        # STATE BOX BOUNDS
        bnd_x_lb = ca.MX(0,0)
        bnd_x_ub = ca.MX(0,0)
        if self.formulation == 'ancillaryonline':
            bnd_z_ub = ca.MX(0,0)
            bnd_z_lb = ca.MX(0,0)
            sampling_step = self.control_interval * 1               # Note: 1 constraint for each control interval
            sampled_z_star_list = self.z_star_list[::sampling_step]
        for j in range(self.nintervals):
            # SLACKNESS ON STATES
            if self.slackness:
                bnd_x_ub_j = self.Xk[:,j] <= self.bnds_x[:,1] + self.eps2
                bnd_x_lb_j = self.Xk[:,j] >= self.bnds_x[:,0] - self.eps1
                # Enforce bounds on nominal trajectories re-computed in the ancillary problem
                # This enforces constraint also on self.z_star0 decision variable (first element of sampled_z_star_list)
                if self.formulation == 'ancillaryonline':
                    bnd_z_ub_j = sampled_z_star_list[j] <= self.bnds_z[:,1]
                    bnd_z_lb_j = sampled_z_star_list[j] >= self.bnds_z[:,0]
            else:
                bnd_x_ub_j = self.Xk[:,j] <= self.bnds_x[:,1]   
                bnd_x_lb_j = self.Xk[:,j] >= self.bnds_x[:,0]
                if self.formulation == 'ancillaryonline':
                    bnd_z_ub_j = sampled_z_star_list[j] <= self.bnds_z[:,1]
                    bnd_z_lb_j = sampled_z_star_list[j] >= self.bnds_z[:,0]
            bnd_x_ub = ca.vertcat(bnd_x_ub,bnd_x_ub_j)
            bnd_x_lb = ca.vertcat(bnd_x_lb,bnd_x_lb_j)
            self.opti.subject_to(bnd_x_lb_j)
            self.opti.subject_to(bnd_x_ub_j)
            if self.formulation == 'ancillaryonline':
                bnd_z_ub = ca.vertcat(bnd_z_ub,bnd_z_ub_j)
                bnd_z_lb = ca.vertcat(bnd_z_lb,bnd_z_lb_j)
                self.opti.subject_to(bnd_z_lb_j)
                self.opti.subject_to(bnd_z_ub_j)
        if self.formulation == 'ancillaryonline':
            self.constraints_dict.update({'bnd_z_ub': bnd_z_ub, 'bnd_z_lb': bnd_z_lb})

        # # Already included in the loop above on sampled_z_star_list
        # if self.formulation == 'ancillaryonline':
        #     bnd_z = self.opti.bounded(self.bnds_z[:,0], self.z_star0, self.bnds_z[:,1])
        #     self.opti.subject_to(bnd_z)
        
        # MOVEBLOCKING (constant control action after Nu control steps i.e. for all steps k > Nu)
        moveblocking = self.Uk[self.nintervals_moveblocking-1:] - self.Uk[self.nintervals_moveblocking-1]
        moveblock = moveblocking == 0
        self.opti.subject_to(moveblock)
        
        # SYSTEM DYNAMICS
        # Calculate the starting index for sampling (corresponding to start_timestamp + {control_interval}h)
        start_index = (self.control_interval - 1) * 1           # {control_interval}h - 1h offset (missing initial value in self.x_list)
        # Sample every {control_interval}hours ({control_interval} * mesh_finesse steps)
        sampling_step = self.control_interval * 1               # Note: 1 constraint for each control interval
        # Sample self.x_list starting from start_index
        sampled_x_list = self.x_list[start_index::sampling_step]
        gaps = ca.horzcat(*sampled_x_list[:-1]) - self.Xk[:,1:]
        close_gaps = gaps == 0
        self.opti.subject_to(close_gaps)
        self.constraints_dict.update({'bnd_u': bnd_u, 'bnd_du': bnd_du, 'bnd_x_ub': bnd_x_ub, 
                                      'bnd_x_lb': bnd_x_lb, 'moveblock': moveblock, 'close_gaps': close_gaps})
        
        # SLACK POSITIVENESS BOUNDS
        if self.slackness:
            bnd_eps1 = self.eps1 >= 0
            self.opti.subject_to(bnd_eps1)
            bnd_eps2 = self.eps2 >= 0
            self.opti.subject_to(bnd_eps2)
            self.constraints_dict.update({'bnd_eps1': bnd_eps1, 'bnd_eps2': bnd_eps2})
        
        # STATE INITIAL CONDITION (not used anymore, see 'stet_init_state' method)
        # close_init_cond_gap = None
        # if self.init_state is not None:
        #     initial_state_cond = self.Xk[:,0] - self.init_state #Set equal to last point of simulator at last MPC step...NOT ANYMORE...SET EQUAL TO EKF!
        #     close_init_cond_gap = initial_state_cond == 0
        #     self.opti.subject_to(close_init_cond_gap)
        #     self.constraints_dict.update({'initial_state_cond': close_init_cond_gap})
        # else: 
        #     self.logger.warning('NMPC initial state condition not set, no initial condition constraint is set.')
    ############################################################################################################

    def additional_ext_contraint(self, constraint_name: str = 'additional', constraint: ca.MX = None):
        '''
        Function to add additional external constraints to the Opti instance (user-defined externally).
        Note: internally defined constraints are added in the 'add_constraints' method.
        Note: constraint must be a symbolic CasADi expression.
        Note: user can use this function to add any kind of custom constraint to the NMPC problem.
        '''
        self.opti.subject_to(constraint)
        self.constraints_dict.update({constraint_name: constraint})
        if constraint_name == 'initial_state_cond':
            self.logger.warning('Initial state condition constraint should not added here, but calling "set_init_state"!')
        # if constraint_name == 'close_gaps':
        #     self.logger.info('Dynamics constraint added.')
    ############################################################################################################

    def setup_solver(self, maxiter: int, print_level: int = 0, file_print_level: int = 5):
        '''
        Function to set up the solver for the Opti instance.
        Args:
            maxiter (int): Maximum number of iterations for the solver.
            print_level (int, optional): Print level for the solver. Defaults to 0 (no output).
            file_print_level (int, optional): File print level for the solver output file. Defaults to 5.

        Note: IPOPT solver is used by default.
        Note: alternatively user can provide a dictionary of options

        '''    
        # CasADi plugin options
        p_opts = {"expand":False}

        # Solver options
        s_opts = {"max_iter": maxiter, 
                  'print_level': print_level,                       # Disable solver output,
                  'output_file': f'{self.modelname}_IPOPT_output.txt',
                  'file_print_level':file_print_level,
                  #"hessian_approximation": "limited-memory"        # 'limited-memory' or 'exact'
                  #'file_append': 'yes',
                }
        self.opti.solver("ipopt",
                         p_opts,s_opts)
    ############################################################################################################

    def set_ext_cost_function(self,u_values_discrete: np.ndarray = None, param_u_values_discrete: np.ndarray = None, delta_t: np.ndarray = None,
                            start_timestamp: Optional[pd.Timestamp] = None, end_timestamp: Optional[pd.Timestamp] = None, 
                            log: bool = False) -> Tuple[ca.MX, Dict]:
        '''
        Function to set the external 'cost_function' for the Opti instance (user-defined externally).
        Args:
            u_values_discrete (np.ndarray): discrete flow-rate input values for the prediction model over the simulation horizon
            param_u_values_discrete (np.ndarray): discrete parameter characterization input values for the prediction model over the simulation horizon
            delta_t (np.ndarray): discrete time step sizes for the prediction model over the simulation horizon. Must be in days!!
            start_timestamp: pd.Timestamp of the start of the prediction horizon
            end_timestamp: pd.Timestamp of the end of the prediction horizon
            log (bool): whether to log information about the cost function setup. Defaults to False.
        Returns:
            cost (ca.MX): symbolic expression of the cost function to be minimized
            costs_dict (Dict): dictionary containing the different cost terms

        Note: user can provide specific inputs to override the default ones set in the 'prepare_inputs' method.
        Note: the results of the 'self.cost_function' call are set as attributes of the NMPC object.
            The symbolic expression of the cost function is returned to be set in the 'opti.minimize' method externally.
        '''
        self.logger.info('## Setting the external cost function...##')
        # Check inputs
        if u_values_discrete is None or param_u_values_discrete is None or delta_t is None:
            self.logger.warning('Inputs not given, using the default values.')
            if self.u_values is None or self.param_u_values is None or self.delta_t is None:
                self.logger.error('Default inputs not set. Use the "prepare_inputs" function to set them.')
                raise ValueError('Default inputs not set. Use the "prepare_inputs" function to set them.')
        u_values_discrete = self.u_values if u_values_discrete is None else u_values_discrete
        param_u_values_discrete = self.param_u_values if param_u_values_discrete is None else param_u_values_discrete
        delta_t = self.delta_t if delta_t is None else delta_t
        #----------------------------------------------------------------------------------------------------------- #
        # Check timestamps consistency
        start = start_timestamp if start_timestamp is not None else self.integrator_parameters['start_timestamp']
        end = end_timestamp if end_timestamp is not None else self.integrator_parameters['end_timestamp']
        time_points_computed = None
        timestamp_points_computed = None
        if start_timestamp is not None and delta_t is not None:
            time_points_computed = np.concatenate(([0], np.cumsum(86400*delta_t)))
            timestamp_points_computed = (start_timestamp + pd.to_timedelta(time_points_computed-time_points_computed[0], unit='S')).round('S')
        time_points_discrete = self.time_points if time_points_computed is None else time_points_computed
        timestamp_points_discrete = self.timestamp_points if timestamp_points_computed is None else timestamp_points_computed
        if end != timestamp_points_discrete[-1]:
            self.logger.warning(f'Timestamps are different: {end} and {timestamp_points_discrete[-1]}.')
        if start + pd.Timedelta(hours=self.control_interval*self.nintervals) != end:
            self.logger.warning(f'{start + pd.Timedelta(hours=self.control_interval*self.nintervals)} is different from {end}.')
            self.logger.info(f'Horizon as given from attribute N is {self.N} days; and {(end-start).total_seconds()/86400} computed from timestamps. Coherent?')
        if log:
            self.logger.info(f'Integrating from {self.integrator_parameters["start_timestamp"]} to {self.integrator_parameters["end_timestamp"]}...consistent?')
            steps = timestamp_points_discrete[(timestamp_points_discrete >= start) & (timestamp_points_discrete<= start + pd.Timedelta(hours=self.control_interval*self.nintervals))][:-1].shape[0]
            self.logger.info(f'Integrating from {time_points_discrete[0]} to {time_points_discrete[-1]} with {steps} steps.')
            self.logger.info(f'Integrating from {timestamp_points_discrete[0]} to {timestamp_points_discrete[-1]} i.e. {(timestamp_points_discrete[-1]-timestamp_points_discrete[0]).total_seconds()/86400} days with {steps} steps.')
        if log:
            self.logger.info(f'Integration step for consistent timestamp and time_points is = {len(u_values_discrete)/(self.nintervals*self.control_interval)}. Ok?')
            self.logger.info(f'Integration step for consistent timestamp and time_points is = {len(u_values_discrete)/((timestamp_points_discrete[-1] - timestamp_points_discrete[0]).total_seconds()/86400*24)}. Ok?')
        #----------------------------------------------------------------------------------------------------------- #
        # Check model CasADi functions (systems and output's evaluation) are set in the model object (needed for the cost function evaluation)
        if self.model.one_step_ahead_ca is None:
            self.model.set_casadi_functions()
        if log:
            self.logger.info(f'The model gas output are in {self.model.other_parameters["udm"]}; check setpoints are in the same units.')
        #----------------------------------------------------------------------------------------------------------- #
        # Set the cost function and call it
        add_kargs = {}
        if self.formulation != 'nominal':
            add_kargs = {'formulation':self.formulation,
                        'v_star':self.v_star,
                        'weights_xstar':self.weights_xstar,
                        'weights_ustar':self.weights_ustar,
                        'weights_yN':self.weights_yN}
            if self.formulation == 'ancillaryonline':
                add_kargs.update({'z_star0':self.z_star0})
            elif self.formulation == 'ancillaryoffline':
                add_kargs.update({'z_star':self.z_star})
        else:
            add_kargs = {'slackness': self.slackness}
            if self.slackness:
                add_kargs.update({'eps1': self.eps1,
                                  'eps2': self.eps2,
                                  'weights_eps1': self.weights_eps1,
                                  'weights_eps2': self.weights_eps2})
        cost, costs_dict, result_timestamps, x_list, y_list, z_star_list, y_star_list = self.cost_function(self.Uk, self.Xk, self.dUk,
                            self.weights_y, self.weights_du,
                            self.model.one_step_ahead_ca, self.model.output_ca,
                            self.state_scales, self.output_scales, self.input_scale,
                            self.ref_df, self.model.process_parameters,
                            self.nintervals, start, end, self.control_interval,
                            u_values_discrete, param_u_values_discrete, delta_t, timestamp_points_discrete,
                            self.integrator_parameters['mesh_finesse'], **add_kargs)
        # Extract symbolic results to be set as attributes
        self.costs_dict = costs_dict
        self.x_list = x_list
        self.y_list = y_list
        self.z_star_list = z_star_list
        self.y_star_list = y_star_list
        # Set the cost function to be minimized in the opti instance
        self.opti.minimize(cost)
        return cost, result_timestamps
    ############################################################################################################

    def set_cost_function(self,
                    u_values_discrete: np.ndarray = None, param_u_values_discrete: np.ndarray = None, delta_t: np.ndarray = None,
                    start_timestamp: Optional[pd.Timestamp] = None, end_timestamp: Optional[pd.Timestamp] = None,
                    log: bool = False
                    ) -> Tuple[ca.MX, List[pd.Timestamp]]:
        '''
        DEPRECATED, use the 'set_ext_cost_function' method instead with the 'cost_function_nominal' function
        to solve the nominal problem.
        Function to set the cost function for the Opti instance (internally defined inside the method itself; for the 'nominal' problem only).
        Args:
            u_values_discrete (np.ndarray): discrete flow-rate input values for the prediction model over the simulation horizon
            param_u_values_discrete (np.ndarray): discrete parameter characterization input values for the prediction model over the simulation horizon
            delta_t (np.ndarray): discrete time step sizes for the prediction model over the simulation horizon. Must be in days!!
            start_timestamp: pd.Timestamp of the start of the prediction horizon
            end_timestamp: pd.Timestamp of the end of the prediction horizon
            log (bool): whether to log information about the cost function setup. Defaults to False.
        Returns:
            cost (ca.MX): symbolic expression of the cost function to be minimized
            timestamps (List[pd.Timestamp]): list of timestamps corresponding to each time step in the prediction horizon
        '''
        start = start_timestamp if start_timestamp is not None else self.integrator_parameters['start_timestamp']
        end = end_timestamp if end_timestamp is not None else self.integrator_parameters['end_timestamp']
        if u_values_discrete is None or param_u_values_discrete is None or delta_t is None:
            self.logger.warning('Inputs not given, using the default values.')
            if self.u_values is None or self.param_u_values is None or self.delta_t is None:
                self.logger.error('Default inputs not set. Use the "prepare_inputs" function to set them.')
                raise ValueError('Default inputs not set. Use the "prepare_inputs" function to set them.')
        u_values_discrete = self.u_values if u_values_discrete is None else u_values_discrete
        param_u_values_discrete = self.param_u_values if param_u_values_discrete is None else param_u_values_discrete
        delta_t = self.delta_t if delta_t is None else delta_t
        # Check timestamps consistency ----
        time_points_computed = None
        timestamp_points_computed = None
        if start_timestamp is not None and delta_t is not None:
            time_points_computed = np.concatenate(([0], np.cumsum(86400*delta_t)))
            timestamp_points_computed = (start_timestamp + pd.to_timedelta(time_points_computed-time_points_computed[0], unit='S')).round('S')
        time_points_discrete = self.time_points if time_points_computed is None else time_points_computed
        timestamp_points_discrete = self.timestamp_points if timestamp_points_computed is None else timestamp_points_computed
        if end != timestamp_points_discrete[-1]:
            self.logger.warning(f'Timestamps are different: {end} and {timestamp_points_discrete[-1]}.')
        if start + pd.Timedelta(hours=self.control_interval*self.nintervals) != end:
            self.logger.warning(f'{start + pd.Timedelta(hours=self.control_interval*self.nintervals)} is different from {end}.')
            self.logger.info(f'Horizon as given from attribute N is {self.N} days; and {(end-start).total_seconds()/86400} computed from timestamps. Coherent?')
        if log:
            self.logger.info(f'Integrating from {self.integrator_parameters["start_timestamp"]} to {self.integrator_parameters["end_timestamp"]}...consistent?')
            steps = timestamp_points_discrete[(timestamp_points_discrete >= start) & (timestamp_points_discrete<= start + pd.Timedelta(hours=self.control_interval*self.nintervals))][:-1].shape[0]
            self.logger.info(f'Integrating from {time_points_discrete[0]} to {time_points_discrete[-1]} with {steps} steps.')
            self.logger.info(f'Integrating from {timestamp_points_discrete[0]} to {timestamp_points_discrete[-1]} i.e. {(timestamp_points_discrete[-1]-timestamp_points_discrete[0]).total_seconds()/86400} days with {steps} steps.')
        if log:
            self.logger.info(f'Integration step for consistent timestamp and time_points is = {len(u_values_discrete)/(self.nintervals*self.control_interval)}. Ok?')
            self.logger.info(f'Integration step for consistent timestamp and time_points is = {len(u_values_discrete)/((timestamp_points_discrete[-1] - timestamp_points_discrete[0]).days*24)}. Ok?')
        # --------------------------------
        # Check model CasADi functions are set in the model object
        if self.model.one_step_ahead_ca is None:
            self.model.set_casadi_functions()
        if log:
            self.logger.info(f'The model gas output are in {self.model.other_parameters["udm"]}; check setpoints are in the same units.')
        # Set the cost function, initialize it
        tracking_cost = 0
        self.x_list = []
        self.y_list = [self.model.output_ca(self.Xk[:,0], ca.DM([value[0] for value in self.model.process_parameters.values()]))]
        current_timestamp = start
        result_timestamps = [current_timestamp]
        time_index_at_k = 0
        for k in range(self.nintervals):
            nsteps_in_interval = timestamp_points_discrete[(timestamp_points_discrete >= current_timestamp) & (timestamp_points_discrete<= current_timestamp + pd.Timedelta(hours=self.control_interval))][:-1].shape[0]
            if nsteps_in_interval != self.integrator_parameters['mesh_finesse']*self.control_interval: #TO B
               self.logger.warning(f'Steps in interval are {nsteps_in_interval}, but they must be {self.integrator_parameters["mesh_finesse"]*self.control_interval}.')
            #--------------------    
            uk = self.Uk[k]
            # Integrate over the interval
            x = self.Xk[:,k]
            #time_index_at_k = k*nsteps_in_interval
            time_index_at_kplus1 = time_index_at_k + nsteps_in_interval
            for i in range(time_index_at_k, time_index_at_kplus1, 1):
                x = self.model.one_step_ahead_ca(x,
                                                ca.vertcat(ca.DM(list(u_values_discrete[i,:-1])),uk),
                                                param_u_values_discrete[i].T,
                                                ca.DM(delta_t[i]),
                                                ca.DM([value[0] for value in self.model.process_parameters.values()]) #When in prediction mode, the parameters are fixed.
                                                )
                y = self.model.output_ca(x,
                                         ca.DM([value[0] for value in self.model.process_parameters.values()]))

            self.x_list.append(x) #Added for Multiple Shooting
            self.y_list.append(y)
            # ------------------
            #Update
            current_timestamp_otherway = current_timestamp + pd.Timedelta(hours=self.control_interval) 
            current_timestamp = timestamp_points_discrete[time_index_at_kplus1]
            if current_timestamp_otherway != current_timestamp:
                self.logger.warning(f'Current timestamp adding control interval is {current_timestamp_otherway}, but adding time_indexes is {current_timestamp}')
            result_timestamps.append(current_timestamp)
            time_index_at_k += nsteps_in_interval
            # ------------------
            if self.ref_df[self.ref_df['Timestamp'] == current_timestamp].empty:
                self.logger.warning(f'Reference data at {current_timestamp} is missing.')
            track_error = ca.vertcat(
                self.weights_y[0]*((y[0]*y[1] - self.ref_df[(self.ref_df['Timestamp']==current_timestamp)].values[0,1])/self.output_scales[0])**2,
                self.weights_y[1]*((y[2]/y[1] - self.ref_df[(self.ref_df['Timestamp']==current_timestamp)].values[0,2])/self.output_scales[1])**2)
            if self.weights_y.shape[0] > 2: #ADD VERTICALLY IF S2,REF IS PRESENT IN Y_REF
                track_error = ca.vertcat(track_error, self.weights_y[2]*((x[6] - self.ref_df[(self.ref_df['Timestamp']==current_timestamp)].values[0,3])/self.output_scales[2])**2)
            # tracking_cost += track_error.T@track_error  # Quadratic cost on control #ADD quadratic cost on FINAL Y ERROR WITH ITS WEIGHT
            tracking_cost += ca.sum1(track_error)  # Quadratic cost on control #ADD quadratic cost on FINAL Y ERROR WITH ITS WEIGHT
            # ------------------
        self.logger.info(f'MPC predicts from {result_timestamps[0]} to {current_timestamp}')
        # Final check of timestamps
        if current_timestamp != end:
            self.logger.warning(f'{current_timestamp} is different from {end}.')
        # Final objective (sum of costs)
        dUk_scaled = self.dUk*ca.repmat(self.input_scale,1,self.nintervals-1)
        controlrate_cost = dUk_scaled@dUk_scaled.T
        cost = self.weights_du*controlrate_cost + tracking_cost #ADD control rate penalty term
        self.costs_dict = {'tracking_cost': tracking_cost, 'controlrate_cost': controlrate_cost}
        if self.slackness:
            slack_ub_cost = ca.sum1((self.eps2/self.state_scales)**2)
            slack_lb_cost = ca.sum1((self.eps1/self.state_scales)**2)
            cost += self.weights_eps1*slack_lb_cost + self.weights_eps2*slack_ub_cost
            self.costs_dict.update({'slack_ub_cost': slack_ub_cost, 'slack_lb_cost': slack_lb_cost})
        self.opti.minimize(cost)
        return cost, result_timestamps
    ############################################################################################################

#--------------------------------------------------------------------------------------------
# These functions are used to attribute values to the parameters/decision variables of the Opti instance.
    def set_init_decvar(self, initial_guesses: Optional[Dict[str, np.ndarray]]):
        """Initialize the Opti instance with the initial state and control.
        Function to set the initial guesses for the decision variables of the Opti instance,
        from the 'initial_guesses' dictionary.
        Args:
            initial_guesses (Dict[str, np.ndarray]): dictionary containing the initial guesses for the decision variables.
                Keys: 'Uk', 'Xk', 'eps1', 'eps2', 'z_star0' (if applicable).
        Note: if key not given as input in the dictionary, set to default values.
        """
        self.logger.info('## Setting initial decision variables...##')
        
        # CONTROLLABLE INPUTS
        if 'Uk' not in initial_guesses:
            self.logger.warning('No initial guess for Uk is provided. Set to zero.')
            self.opti.set_initial(self.Uk, ca.DM(ca.repmat(0,1,self.nintervals)))
        else:
            self.opti.set_initial(self.Uk, ca.DM(initial_guesses.get("Uk")))
        
        # STATES
        if 'Xk' not in initial_guesses:
            self.logger.warning(f'No initial guess for Xk is provided. Set to internal model initial condition = {self.model.init_state}.')
            self.opti.set_initial(self.Xk, ca.DM(ca.repmat(self.model.init_state,1,self.nintervals)))
        else:
            self.opti.set_initial(self.Xk, ca.DM(initial_guesses.get("Xk")))
        
        # SLACKS (if present)
        if self.slackness:
            if 'eps1' not in initial_guesses:
                self.logger.warning('No initial guess for eps1 is provided. Set to 0')
                self.opti.set_initial(self.eps1, ca.DM(ca.repmat(0, self.model.init_state.shape[0], 1)))
            else:
                self.opti.set_initial(self.eps1, ca.DM(initial_guesses.get("eps1")))
            if 'eps2' not in initial_guesses:
                self.logger.warning('No initial guess for eps2 is provided. Set to 0')
                self.opti.set_initial(self.eps2, ca.DM(ca.repmat(0, self.model.init_state.shape[0], 1)))
            else:
                self.opti.set_initial(self.eps2, ca.DM(initial_guesses.get("eps2")))

        # INITIAL VALUES OF THE OPTIMAL NOMINAL STATES UPDATED IN THE ANCILLARY PROBLEM (ONLINE FORMULATION)
        if self.formulation == 'ancillaryonline':
            if 'z_star0' not in initial_guesses:
                self.logger.warning('No initial guess for z_star0 is provided. Set to 0')
                self.opti.set_initial(self.z_star0, ca.DM(ca.repmat(0, self.model.init_state.shape[0], 1)))
            else:
                self.opti.set_initial(self.z_star0, ca.DM(initial_guesses.get("z_star0"))) # Note: In the old codes, it was z_star[:,0] i.e. the nominal solution of the current k^{th} step
    ############################################################################################################

    def set_init_state(self, x0: np.ndarray = None):
        """Set the initial state of the NMPC problem.
        Function to set the initial state of the NMPC problem, thus setting the initial condition constraint in the Opti instance,
        given the multiple shooting formulation.
        Args:
            x0 (np.ndarray): initial state of the NMPC problem. If None, set to the model's default initial state.
        
        Note: if x0 is None, set to the model's default initial state.
        """
        if x0 is None:
            self.init_state = ca.DM(self.model.init_state)
            self.logger.info(f'Initial state of the NMPC set to the default model init_state: {x0}.')
        else:
            self.init_state = ca.DM(x0)
            self.logger.info(f'Initial state of the NMPC set to {x0}.')
            if not np.array_equal(x0, self.model.init_state):
                self.logger.warning(f'Initial state NMPC is different from the model initial state. Check if it is correct.')
                compare_arrays(x0, self.model.init_state)
        state_keys = self.model.get_state_dict().keys()
        self.status.update(dict(zip(state_keys, list(self.init_state.full()[:,0]))))
        
        # Set up initial state constraint (on the Xk decision variable)
        self.logger.info('Setting initial state constraint.')
        initial_state_cond = self.Xk[:,0] - self.init_state #Set equal to last point of simulator at last MPC step...NOT ANYMORE...SET EQUAL TO EKF!
        close_init_cond_gap = initial_state_cond == 0
        self.opti.subject_to(close_init_cond_gap)
        self.constraints_dict.update({'initial_state_cond': close_init_cond_gap})
    ############################################################################################################

    def set_parameters(self, opti_param_values: Dict[str, np.ndarray]):
        """Set the parameters of the Opti instance, from the 'opti_param_values' dictionary.
        Args:
            opti_param_values (Dict[str, np.ndarray]): dictionary containing the values for the parameters
                Keys: 'weights_y', 'weights_du', 'bnds_u', 'bnds_du', 'bnds_x', 'weights_eps1', 'weights_eps2',
                      'weights_xstar', 'weights_ustar', 'weights_yN', 'bnds_z' (if applicable).
        
        Note: if key not given as input in the dictionary, set to default values.
        Note: 
            always: 
                weights_y, weights_du, bnds_u, bnds_du, bnds_x,;
            if slackness: 
                weights_eps1, weights_eps2;
            if problem == ancillary: 
                weights_xstar, weights_ustar, weights_yN;
            if problem == ancillary and formulation == ancillaryonline: 
                bnds_z.
        """
        self.logger.info('## Setting parameters of the Opti instance...##')

        if 'weights_y' not in opti_param_values:
            self.logger.warning('No initial guess for weights_y is provided. Set all to 1.')
            self.opti.set_value(self.weights_y, ca.DM.ones(self.weights_y.shape[0]))
        else:
            self.opti.set_value(self.weights_y, ca.DM(opti_param_values.get("weights_y")))

        if 'weights_du' not in opti_param_values:
            self.logger.warning('No initial guess for weights_du is provided. Set to 0.')
            self.opti.set_value(self.weights_du, 0)
        else:
            self.opti.set_value(self.weights_du, ca.DM(opti_param_values.get("weights_du")))

        if 'bnds_u' not in opti_param_values:
            self.logger.warning('No initial guess for bnds_u is provided. Set to (0,1000).')
            self.opti.set_value(self.bnds_u, ca.DM([[0,1000]]))
        else:
            self.opti.set_value(self.bnds_u, ca.DM(opti_param_values.get("bnds_u")))

        if 'bnds_du' not in opti_param_values:
            self.logger.warning('No initial guess for bnds_du is provided. Set to (-1000,1000).')
            self.opti.set_value(self.bnds_du, ca.DM([[-1000,1000]]))
        else:
            self.opti.set_value(self.bnds_du, ca.DM(opti_param_values.get("bnds_du")))

        if 'bnds_x' not in opti_param_values:
            bnds_x_default = ca.DM.zeros(self.model.init_state.shape[0],2)
            bnds_x_default[:,1] = 100*ca.DM.ones(bnds_x_default.shape[0])
            bnds_x_default[7,:] = ca.DM([-100,100]) #must be changed if I change model!!
            bnds_x_default[-1,1] = 1
            self.logger.warning(f'No initial guess for bnds_x is provided. Set to {bnds_x_default}')
            self.opti.set_value(self.bnds_x, bnds_x_default)
        else:
            self.opti.set_value(self.bnds_x, ca.DM(opti_param_values.get("bnds_x")))

        if self.slackness:
            if 'weights_eps1' not in opti_param_values:
                self.logger.warning('No initial guess for weights_eps1 is provided. Set to 1000.')
                self.opti.set_value(self.weights_eps1, 1000)
            else:
                self.opti.set_value(self.weights_eps1, opti_param_values.get("weights_eps1"))

            if 'weights_eps2' not in opti_param_values:
                self.logger.warning('No initial guess for weights_eps2 is provided. Set to 1000.')
                self.opti.set_value(self.weights_eps2, 1000)
            else:
                self.opti.set_value(self.weights_eps2, opti_param_values.get("weights_eps2"))
        if self.formulation != 'nominal':
            if 'weights_xstar' not in opti_param_values:
                self.logger.warning('No initial guess for weights_xstar is provided. Set to 1.')
                self.opti.set_value(self.weights_xstar, 1)
            else:
                self.opti.set_value(self.weights_xstar, opti_param_values.get("weights_xstar"))

            if 'weights_ustar' not in opti_param_values:
                self.logger.warning('No initial guess for weights_ustar is provided. Set to 1.')
                self.opti.set_value(self.weights_ustar, 1)
            else:
                self.opti.set_value(self.weights_ustar, opti_param_values.get("weights_ustar"))

            if 'weights_yN' not in opti_param_values:
                self.logger.warning('No initial guess for weights_yN is provided. Set to 1.')
                self.opti.set_value(self.weights_yN, 1)
            else:
                self.opti.set_value(self.weights_yN, opti_param_values.get("weights_yN"))
        if self.formulation == 'ancillaryonline':
            if 'bnds_z' not in opti_param_values:
                bnds_z_default = ca.DM.zeros(self.model.init_state.shape[0],2)
                bnds_z_default[:,1] = 100*ca.DM.ones(bnds_z_default.shape[0])
                bnds_z_default[7,:] = ca.DM([-100,100]) # Note: default bound for $\xi$ state! Can be negative. Must be changed if model is changed!!
                bnds_z_default[-1,1] = 1
                self.logger.warning(f'No initial guess for bnds_z is provided. Set to {bnds_z_default}')
                self.opti.set_value(self.bnds_z, bnds_z_default)
            else:
                self.opti.set_value(self.bnds_z, ca.DM(opti_param_values.get("bnds_z")))
    ############################################################################################################
#--------------------------------------------------------------------------------------------
        
    def run_optimization(self, log: bool = False):
        '''
        Function to run the optimization problem and extract the solutions.
        Args:
            log (bool): whether to log information about the optimization run. Defaults to False.
        Returns:
            decvar_star (Dict): dictionary containing the optimal decision variables at the solution.
                Keys: 'x_star', 'u_star', 'eps1_star', 'eps2_star', 'z_star0' (if applicable), 'z_star' (if applicable).
            y_star (np.ndarray): optimal outputs at the solution.
            costs_star (Dict): dictionary containing the different optimal term costs at the solution.
            constraints_star (Dict): dictionary containing the different optimal term constraints at the solution.
        
        Note: if optimization fails, the 'opti.debug' values are extracted instead.
        '''
        self.logger.info('## Running the optimization...##')
        # Check if initial state constraint was set
        if self.init_state is None:
            self.logger.warning('NMPC initial state condition not set, no initial condition constraint is set.')
        # Solve the optimization problem
        try:
            import time
            start= time.time()
            self.solution = self.opti.solve()
            self.logger.info(f'IPOPT iterations are {self.solution.stats()["iter_count"]}')
            stop = time.time()
            duration = stop-start
            self.logger.info(f"Entire duration of optimization is {duration}")
            # EXTRACT SOLUTIONS
            decvar_star = {}
            decvar_star['x_star'] = self.solution.value(self.Xk)
            decvar_star['u_star'] = self.solution.value(self.Uk)
            if self.formulation == 'ancillaryonline':
                decvar_star['z_star0'] = self.solution.value(self.z_star0)
                decvar_star['z_star'] = []
                for i in range(len(self.z_star_list)):
                    decvar_star['z_star'].append(self.solution.value(self.z_star_list[i]))
            y_star = np.array(self.model.output_ca(decvar_star['x_star'], ca.DM([value[0] for value in self.model.process_parameters.values()])))
            costs_star = {}
            constraints_star = {}
            for i, key in enumerate(self.costs_dict):
                costs_star[key] = self.solution.value(self.costs_dict[key])
            for i, key in enumerate(self.constraints_dict):
                constraints_star[key] = self.solution.value(self.constraints_dict[key])
            if self.slackness:
                decvar_star['eps1_star'] = self.solution.value(self.eps1)
                decvar_star['eps2_star']  = self.solution.value(self.eps2)
            if log:
                self.logger.info(f'Costs evaluated at solution are: {costs_star}')
        except RuntimeError as e:
            self.logger.error(f'Optimization failed: {e}')
            duration = None
            # EXTRACT SOLUTIONS
            decvar_star = {}
            decvar_star['x_star'] = self.opti.debug.value(self.Xk)
            decvar_star['u_star'] = self.opti.debug.value(self.Uk)
            if self.formulation == 'ancillaryonline':
                decvar_star['z_star0'] = self.opti.debug.value(self.z_star0)
                decvar_star['z_star'] = []
                for i in range(len(self.z_star_list)):
                    decvar_star['z_star'].append(self.opti.debug.value(self.z_star_list[i]))
            y_star = np.array(self.model.output_ca(decvar_star['x_star'], ca.DM([value[0] for value in self.model.process_parameters.values()])))
            costs_star = {}
            constraints_star = {}
            for i, key in enumerate(self.costs_dict):
                costs_star[key] = self.opti.debug.value(self.costs_dict[key])
            for i, key in enumerate(self.constraints_dict):
                constraints_star[key] = self.opti.debug.value(self.constraints_dict[key])
            if self.slackness:
                decvar_star['eps1_star'] = self.opti.debug.value(self.eps1)
                decvar_star['eps2_star']  = self.opti.debug.value(self.eps2)
            if log:
                self.logger.info(f'Costs evaluated at opti.debug are: {costs_star}')

        # Print the 'status' attribute of the NMPC
        status = {
            'Timestamp': self.integrator_parameters['start_timestamp'],
            'end': self.integrator_parameters['end_timestamp'],
            'run_Timestamp': self.current_date,
            'Control_action': decvar_star['u_star'][0],
            'IPOPT_inter': self.solution.stats()["iter_count"] if self.solution is not None else None,
            'IPOPT_duration': duration if duration is not None else None
        }
        self.status.update(status)
        state_keys = self.model.get_state_dict().keys()
        self.status.update(dict(zip(state_keys, list(decvar_star['x_star'][:,0]))))
        if self.slackness:
            self.status.update(dict(zip([f'eps1_{key}' for key in state_keys], list(decvar_star['eps1_star']))))
            self.status.update(dict(zip([f'eps2_{key}' for key in state_keys], list(decvar_star['eps2_star']))))
        self.status.update(dict(zip([f'{value[0]}_tilde' for value in self.var_couples_list], list(y_star[:,0]))))
        self.status.update(self.ref_df[(self.ref_df['Timestamp']==self.integrator_parameters['start_timestamp'])].drop(columns='Timestamp').to_dict('records')[0])
        self.status.update(costs_star)

        return decvar_star, y_star, costs_star, constraints_star
    ############################################################################################################

    def one_glance_opti_parameters(self):
        '''
        Function to print the current values of the parameters of the Opti instance. For checking purposes.
        Returns:
            opti_parameters (Dict): dictionary containing the current values of the parameters of the Opti instance.
                Keys: 'weights_y', 'weights_du', 'bnds_u', 'bnds_du', 'bnds_x', 'weights_eps1', 'weights_eps2',
                      'weights_xstar', 'weights_ustar', 'weights_yN', 'bnds_z' (if applicable).
        '''
        # Extract current values of the parameters from the Opti instance , collect them in a dictionary and return it
        self.opti_parameters['weights_y'] = self.opti.value(self.weights_y)
        self.opti_parameters['weights_du'] = self.opti.value(self.weights_du)
        self.opti_parameters['bnds_u'] = self.opti.value(self.bnds_u)
        self.opti_parameters['bnds_du'] = self.opti.value(self.bnds_du)
        self.opti_parameters['bnds_x'] = self.opti.value(self.bnds_x)
        if self.slackness:
                self.opti_parameters['weights_eps1'] = self.opti.value(self.weights_eps1)
                self.opti_parameters['weights_eps2'] = self.opti.value(self.weights_eps2)
        if self.formulation != 'nominal':
            self.opti_parameters['weights_xstar'] = self.opti.value(self.weights_xstar)
            self.opti_parameters['weights_ustar'] = self.opti.value(self.weights_ustar)
            self.opti_parameters['weights_yN'] = self.opti.value(self.weights_yN)
        if self.formulation == 'ancillaryonline':
            self.opti_parameters['bnds_z'] = self.opti.value(self.bnds_z)
        return self.opti_parameters
    ############################################################################################################

    def prepare_inputs(self, maize: pd.DataFrame, cowslurry: pd.DataFrame, tomatosauce: pd.DataFrame, 
                       d_flow: pd.DataFrame, u_flow: pd.DataFrame, 
                       mult_factors: List = [1,1,1], t_span_end: float = None,
                       save_to_csv: bool = False, output_path: str = os.getcwd(), start_timestamp: Optional[pd.Timestamp] = None, end_timestamp: Optional[pd.Timestamp] = None, fill_option: List=["zeros", "zeros", "zeros"],
                       sic: List[float] = [0,193,0], log: bool = False) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
        '''
        Function to prepare model inputs before simulation. 
        This function is very case-specific and bug-prone if inputs are not carefully prepared before its call. 
        However, it wraps all the necessary steps to prepare the inputs (i.e. parameters, initial state, influent characterization and flow rates) before simulation.
        Nominally, should be used on the outputs of the 'read_all' function in 'model_read_file.py'.
        From the dataframes and dictionaries read from files, it returns all the necessary inputs in numpy arrays for simulation.
        It filter and convert inputs from agri-AcoDM to AM2HN-like format. 
        It defines initial state, u_values, param_u_values, time_points, timestamp_points, delta_t, for an integration coherent with the 'integrator_parameters'.
        
        Parameters:
            modelname (str): Name of the model (for file saving files).
            maize, cowslurry, tomatosauce, d_flow, u_flow (pd.DataFrame): DataFrames containing input data read from files.
            integrator_parameters (Dict): Dictionary containing integrator parameters.
            mult_factors (List, optional): Multiplicative factors for flow rates. Defaults to [1,1,1].
            t_span_end (float, optional): End time for simulation span. Defaults to None. To be used only if I don't want to simulate until the end of the input data.
            save_to_csv (bool, optional): Whether to save prepared inputs to CSV files. Defaults to False.
            output_path (str, optional): Path to save output files. Defaults to current working directory.
            start (pd.Timestamp, optional): Start timestamp for filtering input data. Defaults to None.
            end (pd.Timestamp, optional): End timestamp for filtering input data. Defaults to None.
            fill_option (List[str], optional): Options for filling missing data in filtering. Defaults to ["zeros", "zeros", "zeros"]. 
                Check 'filter_and_convert_to_time' function for details.
            sic (List[float], optional): Inorganic dissolved carbon content for maize, cowslurry, and tomatosauce. Defaults to [0,193,0].
            log (bool, optional): Whether to log information during preparation. Defaults to False.
        Returns:
            timestamp_points_discrete (pd.DatetimeIndex): Timestamps for discrete time points.
            time_points_discrete (np.ndarray): Discrete time points in days.
            delta_step_discrete (np.ndarray): Time step differences between discrete time points in days.
            u_values_discrete (np.ndarray): Discrete flow rate values.
            param_u_values_discrete (np.ndarray): Discrete parameter matrix values.
        
        Note: if not directly available as data, 'sic' values must be coherent with those computed from the Modelica simulation of each feedstock's conversion model,
            where a change balance given the pH data is enforced to derive the dissolved inorganic carbon content.
            (see BIOGoAlS.agri-AcoDM and BIOGoAlS.Twin manuals for details).
        Note: see also 'prepare_model_inputs' method in the 'model_input.py' file.
        Note: must be the same of the omonimous function in the open-loop model and EKF classes (except for initial states and covariance).
        Note: here in the NMPC class, the function in 'model_input.py' cannot be used directly, because there is no extraction of initial state.
        Note: 'mesh_finesse' in 'integrator_parameters' is used to define a mesh over which the inputs are evaluated. It is expressed in number of points per hour.
            It is strongly recommended to have values inside the input flow rate and feedstock's composition file that does not have any row that has a Timestamp 
            not multiple of (3600/mesh_finesse) seconds from the start timestamp, otherwise unexpected behaviors may occur.
        '''
        self.logger.info('## Preparing inputs for the NMPC...##')
        start = start_timestamp if start_timestamp is not None else self.integrator_parameters['start_timestamp']
        end = end_timestamp if end_timestamp is not None else self.integrator_parameters['end_timestamp']
        
        # Cut input data (and convert the Timestamp column to time)
        d_flow = filter_and_convert_to_time(file_path='/d_flow', df = d_flow,
                                        start_timestamp=start, 
                                        end_timestamp=end,fill_option=fill_option[0], log=log)
        u_flow = filter_and_convert_to_time(file_path='/u_flow', df = u_flow,
                                        start_timestamp=start, 
                                        end_timestamp=end,fill_option=fill_option[0], log=log)
        maize = filter_and_convert_to_time(file_path='/maize', df = maize,
                                        start_timestamp=start, 
                                        end_timestamp=end,fill_option=fill_option[1], log=log)
        cowslurry = filter_and_convert_to_time(file_path='/cowslurry', df = cowslurry,
                                        start_timestamp=start, 
                                        end_timestamp=end,fill_option=fill_option[1], log=log)
        tomatosauce = filter_and_convert_to_time(file_path='/tomatosauce', df = tomatosauce,
                                        start_timestamp=start, 
                                        end_timestamp=end,fill_option=fill_option[1], log=log)
        
        # Note: important if values are less than 1!!
        d_flow[1][:,0] = np.round(d_flow[1][:,0])
        u_flow[1][:,0] = np.round(u_flow[1][:,0])

        # Convert input compositions from agri-AcoDM to AM2HN-like format
        maize_comp = convert_input_composition('maize', maize[1], sic[0], output_path=output_path)
        slurry_comp = convert_input_composition('cowslurry', cowslurry[1], sic[1], output_path=output_path)
        tomato_comp = convert_input_composition('tomatosauce', tomatosauce[1], sic[2], output_path=output_path)
        comps = dict()
        comps.update(maize_comp[1])
        comps.update(slurry_comp[1])
        comps.update(tomato_comp[1])
        
        # Interpolate the input flow rate signal (gFM/day)
        maize_flow = interp1d(d_flow[1][:,0], d_flow[1][:,1], kind='previous', fill_value=(d_flow[1][0,0], d_flow[1][-1,1]), bounds_error=False)
        slurry_flow = interp1d(d_flow[1][:,0], d_flow[1][:,2], kind='previous', fill_value=(d_flow[1][0,0], d_flow[1][-1,2]), bounds_error=False)
        tomato_flow = interp1d(u_flow[1][:,0], u_flow[1][:,1], kind='previous', fill_value=(u_flow[1][0,0], u_flow[1][-1,1]), bounds_error=False)
        flows = dict()
        flows.update({'maize':[maize_flow]})
        flows.update({'cowslurry': [slurry_flow]})
        flows.update({'tomatosauce': [tomato_flow]})

        # Set simulation (discrete integration points) coherent with integrator parameters
        if t_span_end == None:
            t_span = (0, (end-start).total_seconds()/86400)                                                                                                 # Note: time span of the simulation
        else: 
            t_span = (0, t_span_end)
        if log:
            logging.info(f'The timespan of the simulation is {t_span}, but the flow inputs exist for {(max(d_flow[1][0,0]/86400, u_flow[1][0,0]/86400), min(d_flow[1][-1,0]/86400, u_flow[1][-1,0]/86400))} days')
        mesh = np.arange(0,np.round(t_span[1]*86400,0)+3600/self.integrator_parameters['mesh_finesse'],3600/self.integrator_parameters['mesh_finesse'])     # Note: it does not take the initial point...always 3600. 
        time_points_discrete = np.union1d(d_flow[1][:,0], mesh)                                                                                             # Note: union between a 'mesh_finesse' mesh and maize/slurry event points
        time_points_discrete = np.union1d(time_points_discrete, u_flow[1][:,0])                                                                             # Note: union between a 'mesh_finesse' mesh and tomatosauce event points
        time_points_discrete = time_points_discrete[time_points_discrete <= np.round(t_span[1]*86400,0)]                                                    # Note: up to end of simulation
        delta_step_discrete = np.diff(time_points_discrete)                                                                                                 # Note: difference between subsequent points in the 'mesh_finesse' mesh and maize/slurry event points
        # Verify that delta_step_discrete has only equidistant values
        if log:
            self.logger.info(f'Indices with different delta step {np.where(delta_step_discrete != delta_step_discrete[0])[0]}')

        # Compute the input values over the discrete time points
        u_test_discrete = u(time_points_discrete, flows, mult_factors)                                                                                      # Note: it still includes TAN. To exclude TAN take only u(time_points_discrete,V)[:-1]
        param_u_test_discrete = param_u(time_points_discrete, comps)                                                                                        # Note: to be transposed
        u_values_discrete = np.vstack(u_test_discrete).T[:-1,:]                                                                                             # Note 16.01.2025: don't take anymore the last value!!
        param_u_test_discrete = np.array(param_u_test_discrete).T[:-1,:]                                                                                    # Note 16.01.2025: don't take anymore the last value!!
        param_u_values_discrete = param_u_test_discrete
        timestamp_points_discrete = (start + pd.to_timedelta(time_points_discrete-time_points_discrete[0], unit='S')).round('S')
        # Move from seconds to days
        time_points_discrete = time_points_discrete/86400
        delta_step_discrete = delta_step_discrete/86400

        # Save the input values
        if save_to_csv:
            self.logger.info(f'Saving to csv u and param_u files...')
            df = pd.DataFrame(np.column_stack((time_points_discrete[:-1], u_values_discrete)),columns=['Time','maize','cowslurry','tomatosauce'])
            df.insert(0, 'Timestamp', timestamp_points_discrete[:-1])
            df.to_csv(os.path.join(output_path, f'{self.modelname}_u_values.csv'), index=False)
            # save_df_with_check(df, 'u_test_discrete.csv')
            df = pd.DataFrame(np.column_stack((time_points_discrete[:-1], param_u_values_discrete[:,0,:])),columns=['Time','Xh', 'S1', 'S2', 'Z', 'C', 'N'])
            df.insert(0, 'Timestamp', timestamp_points_discrete[:-1])
            df.to_csv(os.path.join(output_path,f'{self.modelname}_param_u_values_maize.csv'), index=False)
            # save_df_with_check(df, 'param_u_test_discrete_maize.csv')
            df = pd.DataFrame(np.column_stack((time_points_discrete[:-1], param_u_values_discrete[:,1,:])),columns=['Time','Xh', 'S1', 'S2', 'Z', 'C', 'N'])
            df.insert(0, 'Timestamp', timestamp_points_discrete[:-1])
            df.to_csv(os.path.join(output_path,f'{self.modelname}_param_u_values_cowslurry.csv'), index=False)
            # save_df_with_check(df, 'param_u_test_discrete_cowslurry.csv')
            df = pd.DataFrame(np.column_stack((time_points_discrete[:-1], param_u_values_discrete[:,2,:])),columns=['Time','Xh', 'S1', 'S2', 'Z', 'C', 'N'])
            df.insert(0, 'Timestamp', timestamp_points_discrete[:-1])
            df.to_csv(os.path.join(output_path,f'{self.modelname}_param_u_values_tomatosauce.csv'), index=False)

        # Set as default attributes
        self.logger.info('Saving default inputs and time attributes...')
        self.u_values = u_values_discrete
        self.param_u_values = param_u_values_discrete
        self.delta_t = delta_step_discrete
        self.time_points = time_points_discrete
        self.timestamp_points = timestamp_points_discrete

        return timestamp_points_discrete, time_points_discrete, delta_step_discrete, u_values_discrete, param_u_values_discrete
    ############################################################################################################