#include "snopt.h"
#include <stdlib.h>
#include <stdio.h>

/* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
/*  toyConstracint   toyObjective    toyInit                                   */
/*  main                                                                       */
/* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */

void toyConstraint
(
 cint   *mode,
 cint   *nncon,
 cint   *nnjac,
 cint   *nejac,
 double *x,
 double *fCon,
 double *gCon,
 cint   *nState,
 char   *cu,
 cint   *lencu,
 double *iu,
 cint   *leniu,
 double *ru,
 cint   *lenru
)
{
  double x0_2 = x[0] * x[0];
  double x0_4 = x0_2 * x0_2;

  double x1_2 = x[1] * x[1];
  double x1_4 = x1_2 * x1_2;

  fCon[0] = x0_2 + x1_2;
  fCon[1] = x0_4 + x1_4;

  gCon[0] = 2.0 * x[0];
  gCon[1] = 4.0 * x0_2 * x[0];

  gCon[2] = 2.0 * x[1];
  gCon[3] = 4.0 * x1_2 * x[1];

}

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

void toyObjective
(
 cint*   mode,
 cint*   nnobj,
 double* x,
 double* fObj,
 double* gObj,
 cint*   nState,
 char*   cu,
 cint*   lencu,
 double* iu,
 cint*   leniu,
 double* ru,
 cint*   lenru
)
{
  double sum = x[0] + x[1] + x[2];
  *fObj = sum * sum;

  sum = 2.0 * sum;
  gObj[0] = sum;
  gObj[1] = sum;
  gObj[2] = sum;

}

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

void toyInit( snProblem* toy )
{
  cint    i;
  double  objective_add =  0.0;
  double  infty         = 1e20;

  double *Jcol;
  cint   *indJ;
  cint   *locJ;

  snSetProblemName     ( toy, "     Toy" );
  snSetOptionString    ( toy, "Cold start          "       );

  snSetOptionInteger   ( toy, "Iterations limit    ",  100 );
  snSetOptionDouble    ( toy, "Optimality tolerance", 1e-4 );

  snSetVariableBounds  ( toy, 0, -infty, infty );
  snSetVariableBounds  ( toy, 1, -infty, infty );
  snSetVariableBounds  ( toy, 2,   0.0 , infty );
  snSetVariableBounds  ( toy, 3,   0.0 , infty );

  snSetConstraintBounds( toy, 0,   2.0 ,  2.0  );
  snSetConstraintBounds( toy, 1,   4.0 ,  4.0  );
  snSetConstraintBounds( toy, 2,   0.0 , infty );
  snSetConstraintBounds( toy, 3, -infty, infty );

  for ( i = 0 ; i < toy->dimension.n  ; i++ ) {
    snSetVariable( toy, i, SN_BASIS_COLD_ELIGIBLE_0, 1.0 );
  }

  for ( i = 0 ; i < toy->dimension.m; i++ ) {
    snSetConstraint( toy, i, SN_BASIS_COLD_ELIGIBLE_0, 0.0 );
    snSetMultiplier( toy, i, 0.0 );
  }

  Jcol = snGetNonzeroValues        ( toy );
  indJ = snGetNonzeroRowIndices    ( toy );
  locJ = snGetNonzeroColumnPointers( toy );

  /* Populate the Jacobian data structure */

  locJ[0] = 1;

  Jcol[0] = 0.0; indJ[0] = 1;
  Jcol[1] = 0.0; indJ[1] = 2;
  Jcol[2] = 2.0; indJ[2] = 3;

  locJ[1] = 4;

  Jcol[3] = 0.0; indJ[3] = 1;
  Jcol[4] = 0.0; indJ[4] = 2;
  Jcol[5] = 4.0; indJ[5] = 3;

  locJ[2] = 7;

  Jcol[6] = 1.0; indJ[6] = 1;
  Jcol[7] = 3.0; indJ[7] = 4;

  locJ[3] = 9;

  Jcol[8] = 1.0; indJ[8] = 2;
  Jcol[9] = 5.0; indJ[9] = 4;

  locJ[4] = 11;

  snSetObjectiveFunction ( toy, toyObjective  );
  snSetConstraintFunction( toy, toyConstraint );
  snSetObjectiveAdd      ( toy, objective_add );

}

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

int main( int argc , char* argv[] )
{
  snProblem toy;

  cint    linear_objective_row          =  4;
  cint    nonzeros                      = 10;
  cint    jacobian_nonzeros             =  4;
  cint    variables                     =  4;
  cint    constraints                   =  4;
  cint    nonlinear_constraints         =  2;
  cint    nonlinear_jacobian_variables  =  2;
  cint    nonlinear_objective_variables =  3;
  double  objective_add                 =  0.0;

  cint    sn_error;
  char   *error_msg;

  /* Allocate snopt workspace and various arrays that define problem toy. */

  sn_error =
    snInit( &toy,
	    variables,
	    constraints,
	    nonzeros,
	    jacobian_nonzeros,
	    nonlinear_constraints,
	    nonlinear_objective_variables,
	    nonlinear_jacobian_variables,
	    linear_objective_row,
            "toyb.out",
            "stdout" );

  if ( sn_error != SN_OK ) {
    snGetError( &toy , &error_msg );
    printf( "%s: error occurred (%d).\n%s", argv[0] , sn_error, error_msg );

    snDelete( &toy );
    free(error_msg);

    exit(EXIT_FAILURE);
  }

  /* Populate the arrays that define problem toy. */
  /* Set some optional parameters.                */

  toyInit( &toy );

  /* Solve problem toy.                           */

  sn_error = snSolve ( &toy );

  if ( sn_error != SN_OK ) {

    snGetError( &toy , &error_msg );
    printf( "%s: error occurred (%d).\n%s", argv[0] , sn_error, error_msg );

    free(error_msg);
    snDelete( &toy );

    exit(EXIT_FAILURE);

  }

  snDelete( &toy );
  exit(EXIT_SUCCESS);

  return 0;

}
