#define MAX_CLIMB_SUM_ERR 10
#define MAX_CLIMB 1

#define CLOCK 16
#define MAX_PPRZ (CLOCK * 600)

#define CLIMB_LEVEL_GAZ 0.31
#define CLIMB_GAZ_OF_CLIMB 0.75
#define CLIMB_PITCH_OF_VZ_PGAIN 0.05
#define CLIMB_PGAIN -0.03
#define CLIMB_IGAIN 0.1
#define NAV_PITCH 0.0

/** PID function INPUTS */
// The user input: target speed in vertical direction
float desired_climb;
// Vertical speed of the UAV detected by GPS sensor
float estimator_z_dot;

/** PID function OUTPUTS */
float desired_gaz;
float desired_pitch;

/** The state variable: accumulated error in the control */
float climb_sum_err = 0;

/** Computes desired_gaz and desired_pitch */
void climb_pid_run()
{
  float err = estimator_z_dot - desired_climb;

  float fgaz = CLIMB_PGAIN * (err + CLIMB_IGAIN * climb_sum_err) +
               CLIMB_LEVEL_GAZ + CLIMB_GAZ_OF_CLIMB * desired_climb;

  float pprz = fgaz * MAX_PPRZ;
  desired_gaz =
    ((pprz >= 0 && pprz <= MAX_PPRZ) ? pprz : (pprz > MAX_PPRZ ? MAX_PPRZ : 0));

  /** pitch offset for climb */
  float pitch_of_vz =
    (desired_climb > 0) ? desired_climb * CLIMB_PITCH_OF_VZ_PGAIN : 0;
  desired_pitch = NAV_PITCH + pitch_of_vz;

  climb_sum_err = err + climb_sum_err;
  if(climb_sum_err > MAX_CLIMB_SUM_ERR)
    climb_sum_err = MAX_CLIMB_SUM_ERR;
  if(climb_sum_err < -MAX_CLIMB_SUM_ERR)
    climb_sum_err = -MAX_CLIMB_SUM_ERR;
}

int main()
{
  while(1)
  {
    /** Non-deterministic input values */
    desired_climb = nondet_float();
    estimator_z_dot = nondet_float();

    /** Range of input values */
    __CPROVER_assume(desired_climb >= -MAX_CLIMB && desired_climb <= MAX_CLIMB);
    __CPROVER_assume(
      estimator_z_dot >= -MAX_CLIMB && estimator_z_dot <= MAX_CLIMB);

    __CPROVER_input("desired_climb", desired_climb);
    __CPROVER_input("estimator_z_dot", estimator_z_dot);

    climb_pid_run();

    __CPROVER_output("desired_gaz", desired_gaz);
    __CPROVER_output("desired_pitch", desired_pitch);
  }
  return 0;
}
