/* Copyright (C) 2006 University of Colorado.
** Written by Daniel M. Webb.
**
**    This file is part of libidp.
**
**    libidp is free software; you can redistribute it and/or modify
**    it under the terms of the GNU General Public License as published by
**    the Free Software Foundation; either version 2 of the License, or
**    (at your option) any later version.
**
**    libidp is distributed in the hope that it will be useful,
**    but WITHOUT ANY WARRANTY; without even the implied warranty of
**    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**    GNU General Public License for more details.
**
**    You should have received a copy of the GNU General Public License
**    along with Foobar; if not, write to the Free Software
**    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/
#include <internal.h>
#include <string.h>
 
// Example IDP program - Park-Ramirez bioreactor
//
// from: 
//    Arun Tholudur and W. Fred Ramirez,
//    "Obtaining smoother singular arc policies using a modified iterative
//    dynamic programming algorithm"
//    International Journal of Control, vol.68, no.5, 1997, pg 1115-1128.
//
//    (above reference has errors, which were fixed by referring to the original
//     paper:)
//
//   Seujung Park and W. Fred Ramirez
//   "Optimal production of secreted protein in fed-batch reactors"
//   AIChE Journal, 1988, v.34, pg.1550-1558.
//

real get_performance(DM_GET_PERFORMANCE_PARAMETERS);
void initialize_states(DM_INITIALIZE_STATES_PARAMETERS);
void integrate_me(DM_INTEGRATE_ME_PARAMETERS);
void test_integrator(DM_INTEGRATOR_PARAMETERS);

#define NUMBER_OF_STATES 5
#define feed_conc 20

#define I_X  IDPStates[0]
#define I_S  IDPStates[1]
#define I_Pm IDPStates[2]
#define I_Pt IDPStates[3]
#define I_V  IDPStates[4]

#define q    IDPControls[0]

#define X  states[0]
#define S  states[1]
#define Pm states[2]
#define Pt states[3]
#define V  states[4]

#define D_X  derivatives[0]
#define D_S  derivatives[1]
#define D_Pm derivatives[2]
#define D_Pt derivatives[3]
#define D_V  derivatives[4]

// =============================================================
void integrate_me(DM_INTEGRATE_ME_PARAMETERS)
{
idp_info_struct *idp_info = integrator->custom_data2;
APD_ASSERT(3, idp_info_struct_check(idp_info));
APD_ASSERT_ALWAYS(q >= 0.0, "q=%g", q);
real derivatives[IS.n_states];
real fe,fp,mu_x,Y;

//printf("X=%g, S=%g, Pm=%g, Pt=%g, V=%g\n", X, S, Pm, Pt, V);

mu_x = 21.87*S / (S + 0.4) / (S + 62.5);
APD_ASSERT_ALWAYS(errno == 0, "mu_x=%g, S=%g", mu_x, S);
fe   = 4.75*mu_x / (0.12 + mu_x);
APD_ASSERT_ALWAYS(errno == 0, "fe=%g, mu_x=%g", fe, mu_x);
//Tholuduor version:
//fe   = 0.12*mu_x / (4.75 + mu_x);
if (S > 50) fp = 0.0;
else        fp = S*exp(-5.0*S) / (0.1 + S);
APD_ASSERT_ALWAYS(errno == 0, "fp=%g, S=%g", fp, S);
//Tholuduor version:
//Y    = 58.75*mu_x*mu_x + 1.71;
Y    = 7.3;

D_Pm = fe*(Pt - Pm) - q/V * Pm;
D_Pt = fp*X - q/V * Pt;
D_X  = mu_x*X - q/V * X;
D_S  = -Y*mu_x*X + q/V * (feed_conc - S);
D_V  = q;
APD_ASSERT_ALWAYS(errno == 0, "after derivatives");
for (uint i = 0; i < NUMBER_OF_STATES; i++)
  {
    APD_ASSERT_ALWAYS(isfinite(derivatives[i]), "derivatives[%d] is not finite", i);
  }

switch (integrator->state.integration_mode)
  {
    case DM_INTEGRATE_SIMPLE:
        for (uint i = 0; i < NUMBER_OF_STATES; i++) state_derivatives[i] = derivatives[i];
        break;
    case DM_INTEGRATE_DAE:
        for (uint i = 0; i < NUMBER_OF_STATES; i++)
            residuals[i] = state_derivatives[i] - derivatives[i];
        break;
    default: APD_DIE("error");
  }
APD_REPORT(5, "state_derivatives[] = [%g, %g, %g, %g, %g]", 
           state_derivatives[0],
           state_derivatives[1], 
           state_derivatives[2], 
           state_derivatives[3], 
           state_derivatives[4]);
}
// =============================================================
void initialize_states(DM_INITIALIZE_STATES_PARAMETERS)
{
APD_PUSH_FUNCTION(5);
printf("Initializing states\n");
Pm = 1.0E-11;
Pt = 1.0E-11;
X  = 1.0;
S  = 5.0;
V  = 1.0;
APD_POP_FUNCTION();
}
// =============================================================
real get_performance(DM_GET_PERFORMANCE_PARAMETERS)
{
idp_info_struct *idp_info = custom_data2;
APD_ASSERT(3, idp_info_struct_check(idp_info));
real performance       = -1.0 * I_Pm * I_V;
return performance;
}
// =============================================================
int main(int argc, char *argv[])
{
#ifdef IDP_DEBUG
apd_object = apd_initialize(0,  /* int report_threshold */
                            5,  /* int assert_threshold */
                            200,  /* int max_function_depth */
                            false,  /* int report_threshold_decreases_with_stack */
                            stdout  /* FILE *error_stream) */);
#endif
idp_info_struct *idp_info       = idp_create_info();
IS.n_states                     = NUMBER_OF_STATES;
IS.n_controls                   = 1;
IS.iteration_report_interval    = 25;
IS.final_time                   = 15.0;
IS.n_stages                     = 50;
IS.n_state_points               = 1;
IS.n_iterations                 = 75;
IS.n_passes                     = 1;
// Use the same random number seed every time for repeatability
IS.random_number_seed           = (uint)0;

IS.stopping.history_iterations           = 10;
IS.stopping.min_performance_improvement  = 0.0;
IS.stopping.stop_performance             = -100;
IS.stopping.no_progress_iterations       = 0;

IS.range.mode                       = IDP_AUTO_RANGE_AVG_DELTA;
IS.range.control_initial            = 1.0;
IS.range.stage_length_initial       = 1.0;
IS.range.first_pass_final_control_range     = 0.05;
IS.range.last_pass_final_control_range      = 0.02;
IS.range.history_iterations         = 30;
IS.range.control_range_factor       = 1.5;
IS.range.stage_length_range_factor  = 2.0;
IS.range.max_auto_contraction       = 0.01;
IS.range.min_relative_control_range = 1E-3;
IS.range.min_relative_stage_length_range = 1E-3;

// Variable stage length settings
// (IDP_FIXED_STAGE_LENGTH, IDP_VARIABLE_STAGES_WITH_CONTROLS, IDP_VARIABLE_STAGES_AFTER_CONTROLS)
IS.variable_stages.mode             = IDP_FIXED_STAGE_LENGTH;
IS.limits.stage_length_low          = 0.01;
IS.limits.stage_length_high         = IS.final_time / IS.n_stages * 2;
IS.variable_stages.fixed_final_time = true;
IS.variable_stages.penalty          = 1.0E3;
IS.variable_stages.initial_shifting = 0;

// Stagewise constant or linear settings
// (IDP_STAGEWISE_CONSTANT, IDP_LINEAR_CONTINUOUS, IDP_LINEAR_DISCONTINUOUS)
IS.stagewise_mode                  = IDP_STAGEWISE_CONSTANT;

// Control point settings
IS.control_points.full_random_number   = 1;
IS.control_points.solo_random_number   = 0;
IS.control_points.extended_range_on    = false;
IS.control_points.pivot_point_number   = 1;
IS.control_points.pivot_point_distance = 1.0;

// Find numerical control sensitivity at each stage
IS.sensitivity.is_on = false;
IS.sensitivity.distance = 0.01;

IS.simulated_annealing_filter.is_on                 = false;
IS.simulated_annealing_filter.initial_temperature   = 0.001;
IS.simulated_annealing_filter.iteration_decrease    = 0.01;

// optimal control filtering to reduce control variation
//idp_set_control_filter_first_order(idp_info, 0.95/*filter factor*/);

// control damping to smooth by punishing control action
// deprecated:
//idp_set_control_damping(idp_info, 0/*start_iteration*/, 0/*full_iteration*/);

IS.user_functions.initialize_states             = initialize_states;
IS.user_functions.get_performance               = get_performance;
// These are the IDP defaults:
IS.user_functions.save_optimal_control          = idp_save_optimal_control;
IS.user_functions.progress_report               = idp_default_progress_report;

// Set up integrator
IS.integrator.integrate_me                      = integrate_me;
IS.integrator.settings.integrator               = DM_EULER;
IS.integrator.custom_data2                      = idp_info;  
IS.integrator.settings.n_equations              = IS.n_states;
IS.integrator.settings.max_function_evaluations = 1E5;
IS.integrator.settings.max_steps                = 1E4;

IS.integrator.settings.euler.stepsize           = 0.05;

IS.integrator.settings.rk45.relative_tolerance  = 1.0E-5;
IS.integrator.settings.rk45.absolute_tolerance  = 1.0E-5;

IS.integrator.settings.daskr.relative_tolerance = 1.0E-5;
IS.integrator.settings.daskr.absolute_tolerance = 1.0E-5;
IS.integrator.settings.daskr.dont_cross_zero    = true;

// Allocate IDP data structures
idp_allocate_info(idp_info);

// Limits should be set after idp_allocate_info()
IS.limits.control_low [0] = 0.0;
IS.limits.control_high[0] = 2.0;

idp_optimal_control_by_idp(idp_info);
printf("saving optimal controls to test_controls.dat\n");
strcpy(IS.optimal_control_save_filename, "test_controls.dat");
IS.user_functions.save_optimal_control(idp_info, OptimalU, OptimalSL, IDP_OPTIMAL_CONTROL);
idp_free_info(idp_info);
return 0;
}

