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

/* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
/*  toyUser     toyInit                       */
/*  main                                                                       */
/* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */

void toyfun(int mode, int nnobj, int nncon, int nnjac, int nnL,
	    int nejac, /*neH*/ double *x, double *fObj, double *gObj,
	    double yObj, double *fCon, double *gCon, double *yCon,
	    int nState);

void hvprod( int nnH, /*int neH, int nlocH, int* locH,
	    int* indH, double* H,*/ double* v, double* Hv );


static double *H;
static int    *indH, *locH;
static int neH = 6;

/*
void toyConstraint
(
 int    *mode,
 int    *nncon,
 int    *nnjac,
 int    *nejac,
 double *x,
 double *fCon,
 double *gCon,
 int    *nState,
 char   *cu,
 int    *lencu,
 double *iu,
 int    *leniu,
 double *ru,
 int    *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
(
 int*    mode,
 int*    nnobj,
 double* x,
 double* fObj,
 double* gObj,
 int*    nState,
 char*   cu,
 int*    lencu,
 double* iu,
 int*    leniu,
 double* ru,
 int*    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 toyUser
(
 int    *mode,
 int    *nnobj,
 int    *nncon,
 int    *nnjac,
 int    *nnL,
 int    *nejac,
 double *x,
 double *v,
 double *fObj,
 double *gObj,
 double *yObj,
 double *fCon,
 double *gCon,
 double *yCon,
 double *Hv,
 int    *nState,
 char   *cu,
 int    *lencu,
 double *iu,
 int    *leniu,
 double *ru,
 int    *lenru
)
{

  /*int ilindH, illocH, ineh, lh, lindH, llocH, neH, nlocH;*/

 if (*nState == 1) {
   /* Relax */
 }

 /* nlocH = *nnL + 1;

 ineH   = 0;
 illocH = 1;
 ilindH = 2;

 neH   = iu[ineH];
 llocH = iu[illocH];
 lindH = iu[ilindH];

 lH = 0;*/

 /*printf("Good so far!d");
   exit(EXIT_FAILURE);*/

 if (*mode < 4) {
   toyfun(*mode, *nnobj, *nncon, *nnjac, *nnL,
	  *nejac, /*neH*/ x, fObj, gObj, *yObj,
	  fCon, gCon, yCon, *nState);
 }
 else {
   hvprod(*nnL, /*neH, nlocH, &iu[llocH], &iu[lindH], &ru[lH],*/
	  v, Hv );
 }
}


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

void hvprod
(
 int nnH, /*int neH, int nlocH, int* locH,
	    int* indH, double* H,*/ double* v, double* Hv
)
{

int i, j, l;
double sum, vj;

if ( nnH == 0 || neH == 0 ) return;

for ( i=1; i<=nnH; i++ ) {
  sum = 0.0;
  for ( l=locH[i-1]; l<=locH[i]-1; l++  ) {
    j = indH[l-1];
    sum = sum + H[l-1] * v[j-1];
  }
  Hv[i-1] = sum;
}

 for ( j=1; j<= nnH; j++ ) {
   vj = v[j-1];
   for ( l = locH[j-1]; l<= locH[j]-1; l++ ) {
     i = indH[l-1];
     if (i != j) {
       Hv[i-1] = Hv[i-1] + H[l-1]*vj;
     }
   }
 }

}

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


void toyfun
(
 int mode, int nnObj, int nnCon, int nnJac, int nnL, int negCon,
 /*int neHess,*/ double*  x, double* fObj, double* gObj, double yObj,
 double* fCon, double* gCon, double* yCon, int nState
)
{

  int    neG, neHess;
  int    needf=0, needg=0, needH=0;
  double sum;

  if ( mode==0 || mode==2 )
    needf = 1;

  if ( mode==1 || mode==2 )
    needg = 1;

  if ( mode==3 )
    needH = 1;

  sum = x[0] + x[1] + x[2];

  if (needf) {
    fCon[0] =  x[0]*x[0] + x[1]*x[1];
    fCon[1] =              x[1]*x[1]*x[1]*x[1];
    *fObj     =  sum*sum;
  }

  neG = 0;

  if (needg==1) {
  /* -------------------------------------------
     Nonlinear elements for column 1 (g=df/dx1).
     g rows = (1).
     ------------------------------------------- */
    neG = neG + 1;
    gCon[neG-1] =  2.0* x[0];        /* row  1 */
    gObj[0] =  2.0*sum;
  }

  neHess = 0;

  if (needH==1) {

    /* H(1,1) = yObj*df5/dx1 + y1*df1/dx1     !objective is f5,  ie. row 5 */

    neHess    =  neHess + 1;
    /* H(neHess) = two*( one + yCon(1) ) */
    H[neHess-1] =  2.0*(yObj + yCon[0]);

    /* H(1,2) =  yObj*df5/dx2   !objective is f5,  i.e. row 5 of f. */

    neHess    =  neHess + 1;
    /* H(neHess) = two */
    H[neHess-1] = 2.0*yObj;

    /* H(1,3) =  yObj*df5/dx3   !objective is f5,  i.e. row 5 of f. */

    neHess    = neHess + 1;
    /* H(neHess) = two */
    H[neHess-1] = 2.0*yObj;
  }

  /* -------------------------------------------
     Nonlinear elements for column 2 (g=df/dx2).
     g Rows = (1,2).
     ------------------------------------------- */
  if (needg==1) {

    neG = neG + 1;
    gCon[neG-1] =  2.0*x[1];  /* row 1 */
    neG = neG + 1;
    gCon[neG-1] =  4*x[1]*x[1]*x[1]; /* row 2 */

    gObj[1]  =  2.0*sum;
  }

  if (needH==1) {

    /* H(2,2) = yObj*df5/dx2 + y1*df1/dx2 + y2*df2/dx2 */

    neHess    =  neHess + 1;
    H[neHess-1] =  2.0*( yObj + yCon[0] + 6.0*x[1]*x[1]*yCon[1] );
    /* H(neHess) = two*( yObj + yCon(1) ) + 12.0d+0 * x(2) * x(2) * yCon(2) ) */

    /* H(2,3) =  yobj*df5/dx3  !objective is row 5 of f. */

    neHess    =  neHess + 1;
    /* H(neHess) = two */
    H[neHess-1] = 2.0*yObj;
  }

  /* -------------------------------------------
     Nonlinear elements for column 3 (g=df/dx3).
     g Rows = (3,7,10,11,15).
     ----------------------------------------- */

  /*     No nonlinear terms in column 3 of the Jacobian. */
  if (needg==1) {
    gObj[2] = 2.0*sum;
  }

  if (needH==1) {

    /* H(3,3) = yObj*df5/dx3 */

    neHess    =  neHess + 1;
    /* H(neHess) = two */
    H[neHess-1] =  2.0*yObj;
  }

}


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

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

  double *Jcol;
  int    *indJ;
  int    *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;
  locH[0] = 1;

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

  locJ[1] = 3;
  locH[1] = 4;

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

  locJ[2] = 6;
  locH[2] = 6;

  Jcol[5] = 1.0; indJ[5] = 1;
  Jcol[6] = 3.0; indJ[6] = 4;
  indH[5] = 3;

  locJ[3] = 8;
  locH[3] = 7;

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

  locJ[4] = 10;
  /*
  snSetObjectiveFunction ( toy, toyObjective  );
  snSetConstraintFunction( toy, toyConstraint );
  */
  snSetUserFunction( toy, toyUser );
  snSetObjectiveAdd      ( toy, objective_add );

}

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

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

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

  int     nonzeros_hessian              =  6;
  int     hessian_size                  =  3;

  int     sn_error;
  char   *error_msg;

  H    = (double*)malloc( sizeof(double) * nonzeros_hessian );
  indH = (int*)   malloc( sizeof(int)    * nonzeros_hessian );
  locH = (int*)   malloc( sizeof(int)    * (hessian_size+1) );

  /* 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,
	    nonzeros_hessian,
	    hessian_size,
            "toych.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;

}

