/* Copyright (C) 2005, 2006 International Business Machines and others.
 * All Rights Reserved.
 * This code is published under the Common Public License.
 *
 * $Id: hs071_c.c 1324 2008-09-16 14:19:26Z andreasw $
 *
 * Authors:  Carl Laird, Andreas Waechter     IBM    2005-08-17
 */
#include "IpStdCInterface.h"
#include <stdlib.h>
#include <assert.h>
#include <stdio.h>
#include <math.h>

/* Function Declarations */
Bool eval_f(Index n, Number* x, Bool new_x,
            Number* obj_value, UserDataPtr user_data);

Bool eval_grad_f(Index n, Number* x, Bool new_x,
                 Number* grad_f, UserDataPtr user_data);

Bool eval_g(Index n, Number* x, Bool new_x,
            Index m, Number* g, UserDataPtr user_data);

Bool eval_jac_g(Index n, Number *x, Bool new_x,
                Index m, Index nele_jac,
                Index *iRow, Index *jCol, Number *values,
                UserDataPtr user_data);

Bool eval_h(Index n, Number *x, Bool new_x, Number obj_factor,
            Index m, Number *lambda, Bool new_lambda,
            Index nele_hess, Index *iRow, Index *jCol,
            Number *values, UserDataPtr user_data);

/* Main Program */

int main()
{
  Index n=-1;                          /* number of variables */
  Index m=-1;                          /* number of constraints */
  Number* x_L = NULL;                  /* lower bounds on x */
  Number* x_U = NULL;                  /* upper bounds on x */
  Number* g_L = NULL;                  /* lower bounds on g */
  Number* g_U = NULL;                  /* upper bounds on g */
  IpoptProblem nlp = NULL;             /* IpoptProblem */
  enum ApplicationReturnStatus status; /* Solve return code */
  Number* x = NULL;                    /* starting point and solution vector */
  Number* mult_x_L = NULL;             /* lower bound multipliers
  					  at the solution */
  Number* mult_x_U = NULL;             /* upper bound multipliers
  					  at the solution */
  Number obj;                          /* objective value */
  Index i;                             /* generic counter */

  /* Number of nonzeros in the Jacobian of the constraints */
  Index nele_jac = 18;//attention
  /* Number of nonzeros in the Hessian of the Lagrangian (lower or
     upper triangual part only) */
  Index nele_hess = 12;//attention
  /* indexing style for matrices */
  Index index_style = 0; /* C-style; start counting of rows and column
  			    indices at 0 */
  double PI=3.141592653;
  /* set the number of variables and allocate space for the bounds */
  n=12;
  x_L = (Number*)malloc(sizeof(Number)*n);
  x_U = (Number*)malloc(sizeof(Number)*n);
  /* set the values for the variable bounds */
  for (i=0; i<2; i++) {
    x_L[i] = -PI/2;
    x_U[i] = PI/2;
  }
  for (i = 2; i<4; i++) {
	  x_L[i] = 0.95;
	  x_U[i] = 1.05;
  }
  for (i = 4; i<6; i++) {
	  x_L[i] = 0.9;
	  x_U[i] = 1.1;
  }
  for (i = 6; i<12; i++) {
	  x_L[i] = 0;
	  x_U[i] = pow(10,7);
  }

  /* set the number of constraints and allocate space for the bounds */
  m=3;
  g_L = (Number*)malloc(sizeof(Number)*m);
  g_U = (Number*)malloc(sizeof(Number)*m);
  /* set the values of the constraint bounds */
  g_L[0] = 0;
  g_U[0] = 0;
  g_L[1] = 0;
  g_U[1] = 0;
  g_L[2] = 0.1;
  g_U[2] = 0.3099;
  /* create the IpoptProblem */
  nlp = CreateIpoptProblem(n, x_L, x_U, m, g_L, g_U, nele_jac, nele_hess,
                           index_style, &eval_f, &eval_g, &eval_grad_f,
                           &eval_jac_g, &eval_h);

  /* We can free the memory now - the values for the bounds have been
     copied internally in CreateIpoptProblem */
  free(x_L);
  free(x_U);
  free(g_L);
  free(g_U);

  /* Set some options.  Note the following ones are only examples,
     they might not be suitable for your problem. */
  //attention
  AddIpoptNumOption(nlp, "tol", 1e-7);
  AddIpoptStrOption(nlp, "mu_strategy", "adaptive");
  AddIpoptStrOption(nlp, "output_file", "ipopt.out");

  /* allocate space for the initial point and set the values */
  x = (Number*)malloc(sizeof(Number)*n);
  for (i = 0; i<2; i++)
	  x[i] = 0;
  for (i = 2; i<4; i++)
	  x[i] = 1;
  for (i = 4; i<6; i++) 
	  x[i] = 1.026087;
  for (i = 6; i<12; i++)
	  x[i] = 0;
  /* allocate space to store the bound multipliers at the solution */
  mult_x_L = (Number*)malloc(sizeof(Number)*n);
  mult_x_U = (Number*)malloc(sizeof(Number)*n);

  /* solve the problem */
  status = IpoptSolve(nlp, x, NULL, &obj, NULL, mult_x_L, mult_x_U, NULL);

  if (status == Solve_Succeeded) {
    printf("\n\nSolution of the primal variables, x\n");
    for (i=0; i<n; i++) {
      printf("x[%d] = %e\n", i, x[i]);
    }

    printf("\n\nSolution of the bound multipliers, z_L and z_U\n");
    for (i=0; i<n; i++) {
      printf("z_L[%d] = %e\n", i, mult_x_L[i]);
    }
    for (i=0; i<n; i++) {
      printf("z_U[%d] = %e\n", i, mult_x_U[i]);
    }

    printf("\n\nObjective value\n");
    printf("f(x*) = %e\n", obj);
  }

  /* free allocated memory */
  FreeIpoptProblem(nlp);
  free(x);
  free(mult_x_L);
  free(mult_x_U);

  return 0;
}


/* Function Implementations */
Bool eval_f(Index n, Number* x, Bool new_x,
            Number* obj_value, UserDataPtr user_data)
{
  assert(n == 12);

  *obj_value = pow(10, 6)*(x[6] + x[7] + x[8] + x[9] + x[10] + x[11]);

  return TRUE;
}

Bool eval_grad_f(Index n, Number* x, Bool new_x,
                 Number* grad_f, UserDataPtr user_data)
{
  int i;
  assert(n == 12);
  for(i=0;i<6;i++)
	  grad_f[i]=0;
  for (i = 6; i < 12; i++)
	  grad_f[i] = pow(10, 6);
  return TRUE;
}

Bool eval_g(Index n, Number* x, Bool new_x,
            Index m, Number* g, UserDataPtr user_data)
{
  assert(n == 12);
  assert(m == 3);
  g[0] = (-x[2] * cos(x[0])) / (0.075556*x[4]) + pow(x[2], 2) / 0.075556 + x[6] - x[7];
  g[1] = (-x[3] * cos(x[1])) / (0.075556*x[5]) + pow(x[3], 2) / 0.075556 + x[8] - x[9];
  g[2] = -x[2] * cos(x[0]) / (0.075556*x[4]) - x[3] * cos(x[1]) / (0.075556*x[5]) + 1 / 0.075556*(1 / pow(x[4], 2) + 1 / pow(x[5], 2)) + x[10] - x[11];
 
  return TRUE;
}

Bool eval_jac_g(Index n, Number *x, Bool new_x,
                Index m, Index nele_jac,
                Index *iRow, Index *jCol, Number *values,
                UserDataPtr user_data)
{
  if (values == NULL) {
    /* return the structure of the jacobian */

    /* this particular jacobian is dense */
    iRow[0] = 0;
    jCol[0] = 0;
    iRow[1] = 0;
    jCol[1] = 2;
    iRow[2] = 0;
    jCol[2] = 4;
    iRow[3] = 0;
    jCol[3] = 6;
    iRow[4] = 0;
    jCol[4] = 7;
	iRow[5] = 1;
	jCol[5] = 1;
	iRow[6] = 1;
	jCol[6] = 3;
	iRow[7] = 1;
	jCol[7] = 5;
	iRow[8] = 1;
	jCol[8] = 8;
	iRow[9] = 1;
	jCol[9] = 9;
    iRow[10] = 2;
    jCol[10] = 0;
    iRow[11] = 2;
    jCol[11] = 1;
    iRow[12] = 2;
    jCol[12] = 2;
	iRow[13] = 2;
	jCol[13] = 3;
	iRow[14] = 2;
	jCol[14] = 4;
	iRow[15] = 2;
	jCol[15] = 5;
	iRow[16] = 2;
	jCol[16] = 10;
	iRow[17] = 2;
	jCol[17] = 11;
  }
  else {
    /* return the values of the jacobian of the constraints */

    values[0] = (x[2] * sin(x[0])) / (0.075556*x[4]); /* 0,0 */
    values[1] = (-cos(x[0])) / (0.075556*x[4]) + 2*x[2]/ 0.075556; /* 0,2 */
    values[2] = (x[2] * cos(x[0])) / (0.075556*pow(x[4],2)); /* 0,4 */
    values[3] = 1;  /* 0,6 */
	values[4] = -1;	/* 0,7 */

	values[5] = (x[3] * sin(x[1])) / (0.075556*x[5]); /* 1,1 */
	values[6] = (-cos(x[1])) / (0.075556*x[5]) + 2 * x[3] / 0.075556; /* 1,3 */
	values[7] = (x[3] * cos(x[1])) / (0.075556*pow(x[5], 2)); /* 1,5 */
	values[8] = 1;  /* 1,8 */
	values[9] = -1;	/* 1,9 */

	values[10] =x[2] * sin(x[0]) / (0.075556*x[4]); /* 2,0 */
	values[11] =x[3] * sin(x[1]) / (0.075556*x[5]); /* 2,1 */
	values[12] =-cos(x[0]) / (0.075556*x[4]); /* 2,2 */
	values[13] =-cos(x[1]) / (0.075556*x[5]);  /* 2,3 */
	values[14] = x[2] * cos(x[0]) / (0.075556*pow(x[4],2))- 2 / (0.075556*pow(x[4], 3));	/* 2,4 */
	values[15] = x[3] * cos(x[1]) / (0.075556*pow(x[5],2))- 2 / (0.075556*pow(x[5], 3));   /* 2,5 */
	values[16] = 1;  /* 2,10 */
	values[17] = -1;	/* 2,11 */
  }

  return TRUE;
}

Bool eval_h(Index n, Number *x, Bool new_x, Number obj_factor,
            Index m, Number *lambda, Bool new_lambda,
            Index nele_hess, Index *iRow, Index *jCol,
            Number *values, UserDataPtr user_data)
{
  Index idx = 0; /* nonzero element counter */
  Index row = 0; /* row counter for loop */
  Index col = 0; /* col counter for loop */
  int i;
  if (values == NULL) {
    /* return the structure. This is a symmetric matrix, fill the lower left
     * triangle only. */

    /* the hessian for this problem is actually dense */
	iRow[0] = 0;
	jCol[0] = 0;
	iRow[1] = 1;
	jCol[1] = 1;
	iRow[2] = 2;
	jCol[2] = 0;
	iRow[3] = 2;
	jCol[3] = 2;
	iRow[4] = 3;
	jCol[4] = 1;
	iRow[5] = 3;
	jCol[5] = 3;
	iRow[6] = 4;
	jCol[6] = 0;
	iRow[7] = 4;
	jCol[7] = 2;
	iRow[8] = 4;
	jCol[8] = 4;
	iRow[9] = 5;
	jCol[9] = 1;
	iRow[10] = 5;
	jCol[10] = 3;
	iRow[11] = 5;
	jCol[11] = 5;

  }
  else {
    /* return the values. This is a symmetric matrix, fill the lower left
     * triangle only */
    for(i=0;i<12;i++)
	{
		values[i]=0;
	}

    /* add the portion for the first constraint */
    values[0] += lambda[0] * (x[2] * cos(x[0])) / (0.075556*x[4]);          /* 0,0 */
	values[2] += lambda[0] * (sin(x[0])) / (0.075556*x[4]);	 /* 2,0 */
    values[3] += lambda[0] * (2 / 0.075556);          /* 2,2 */
    values[6] += lambda[0] * (-x[2] * sin(x[0])) / (0.075556*pow(x[4], 2));          /* 4,0 */
	values[7] += lambda[0] * (cos(x[0])) / (0.075556*pow(x[4], 2));		 /* 4,2 */
	values[8] += lambda[0] * (-2*x[2] * cos(x[0])) / (0.075556*pow(x[4], 3)); 		 /* 4,4 */

    /* add the portion for the second constraint */
	values[1] += lambda[1] * (x[3] * cos(x[1])) / (0.075556*x[5]);          /* 1,1 */
	values[4] += lambda[1] * (sin(x[1])) / (0.075556*x[5]);	 /* 3,1 */
	values[5] += lambda[1] * (2 / 0.075556);          /* 3,3 */
	values[9] += lambda[1] * (-x[3] * sin(x[1])) / (0.075556*pow(x[5], 2));          /* 5,1 */
	values[10] += lambda[1] * (cos(x[1])) / (0.075556*pow(x[5], 2));		 /* 5,3 */
	values[11] += lambda[1] * (-2*x[3] * cos(x[1])) / (0.075556*pow(x[5], 3)); 		 /* 5,5 */

	/* add the portion for the third constraint */
	values[0] += lambda[2] * x[2] * cos(x[0]) / (0.075556*x[4]);          /* 0,0 */
	values[1] += lambda[2] * x[3] * cos(x[1]) / (0.075556*x[5]);	 /* 1,1 */
	values[2] += lambda[2] * sin(x[0]) / (0.075556*x[4]);          /* 2,0 */
	values[4] += lambda[2] * sin(x[1]) / (0.075556*x[5]);          /* 3,1 */
	values[6] += lambda[2] * (-x[2] * sin(x[0])) / (0.075556*pow(x[4], 2));		 /* 4,0 */
	values[7] += lambda[2] * cos(x[0]) / (0.075556*pow(x[4], 2)); 		 /* 4,2 */
	values[8] += lambda[2] * (-2*x[2] * cos(x[0]) / (0.075556*pow(x[4], 3)) + 6 / (0.075556*pow(x[4], 4)));    /* 4,4 */
	values[9] += lambda[2] * (-x[3] * sin(x[1])) / (0.075556*pow(x[5], 2));		 /* 5,1 */
	values[10] += lambda[2] * cos(x[1]) / (0.075556*pow(x[5], 2)); 		 /* 5,3 */
	values[11] += lambda[2] * (-2*x[3] * cos(x[1]) / (0.075556*pow(x[5], 3)) + 6 / (0.075556*pow(x[5], 4)));    /* 5,5 */
  }

  return TRUE;
}