/*
 *  traj.c
 *  
 *
 *  Created by Rakshit on 15/02/06.
 *  Copyright 2006 __MyCompanyName__. All rights reserved.
 *
 */



#include "IpStdCInterface.h"
#include "assert.h"
#include "stdio.h"
#include "stdlib.h"
#include "math.h"

inline Number square(Number x) {return x * x;}

/*Declaring functions*/

/* function for evaluating objective function*/
Bool eval_f(Index n, Number* x, Bool new_x,Number* obj_value, UserDataPtr data);

/*function for evaluating gradient of objective function*/
Bool eval_grad_f(Index n, Number* x, Bool new_x,Number* grad_f, UserDataPtr data);

/*function for evaluating constraint function*/
Bool eval_g(Index n, Number* x, Bool new_x,Index m, Number* g, UserDataPtr data);

/*function for evaluating jacobian of Constraint function*/
Bool eval_jac_g(Index n, Number *x, Bool new_x,Index m, Index nele_jac,Index *iRow, Index *jCol, Number *values,UserDataPtr data);

/*fucntion for evaluating hessian of the langrangian function*/
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 data);
typedef struct 
 {
  Number a,b,c;
  Number StartVert[2][3];
  Number EndVert[2][3];
 }Udata;

int main()
 {   
	Index n=-1;
	Index m=-1;
	Number* xu=NULL;
	Number* xl=NULL;
	Number* gu=NULL;
	Number* gl=NULL;
	IpoptProblem myprob=NULL;
	enum ApplicationReturnStatus status;
	Number g,s;
	UserDataPtr data;
	Udata *v;
	
		
	Number* x=NULL;
	Number* multxu=NULL;
	Number* multxl=NULL;
	
	Number obj;
	Index i; 
	
	v= (Udata*)malloc(sizeof(Udata));
	v->a=1;
	v->b=1;
	v->c=1;
	v->StartVert[0][0]=0;
	v->StartVert[0][1]=0;
	v->StartVert[0][2]=0;
	
	v->StartVert[1][0]=0;     
	v->StartVert[1][1]=0;
	v->StartVert[1][2]=0;
	
	v->EndVert[0][0]=0;
	v->EndVert[0][1]=0;
	v->EndVert[0][2]=0;
	
	v->EndVert[1][0]=0;
	v->EndVert[1][1]=0;
	v->EndVert[1][2]=0;
	
	
	data=v;
	/*printf(" data a of v is %f ",((Udata*)data)->a);*/
	
	
	printf("\n enter the no. of points ");
	scanf("%d",&n);
	printf("\n n is %d ",n);
	/*n=3;*/
		
	xu = (Number*)malloc(sizeof(Number)*3*n);
	xl = (Number*)malloc(sizeof(Number)*3*n);
	
	
	for(i=0;i<3*n;i++)
	 {
	  xl[i]=-2e19;
	  xu[i]=2e19;
	 }
	 
	printf("\n enter the upper bounds of constraints G and S ");
	scanf("%lf %lf",&g,&s);
	/*g=5;
	s=5;*/
	
	m=2*n+3;
	gu = (Number*)malloc(sizeof(Number)*m);
    gl = (Number*)malloc(sizeof(Number)*m);
	
	gl[0]=-2e19; gu[0]=g*g;
	gl[1]=-2e19; gu[1]=g*g;
	gl[2]=-2e19; gu[2]=s*s;
	gl[3]=-2e19; gu[3]=s*s;
	gl[4]=-2e19;gu[4]=s*s;
	gl[5]=-2e19;gu[5]=s*s;
	
	for(i=0;i<n-1;i++)
	 {
	   gl[i+6]=-2e19; gu[i+6]=g*g;
	 }  
	for(i=0;i<n-2;i++)
	 {
	  gl[i+6+(n-1)]=-2e19;gu[i+6+(n-1)]=s*s;
	 }
	
	/*Creating IPOPT Problem*/
	myprob = CreateIpoptProblem(3*n,xl,xu,m,gl,gu,15*n,9*n-9,0,&eval_f,&eval_g,&eval_grad_f,&eval_jac_g,&eval_h);

	
	free(xl);
	free(xu);
	free(gl);
	free(gu);
	
	//AddIpoptNumOption(myprob, "tol", 1e-9);
    //AddIpoptStrOption(myprob, "mu_strategy", "adaptive");
	
	x=(Number*)malloc(sizeof(Number)*3*n);
	for(i=0;i<3*n;i++)
	 {
	  x[i]=0;/*random()/(pow(2,31)-1);*/
	 }
	 
	 	
	
	multxl = (Number*)malloc(sizeof(Number)*3*n);
    multxu = (Number*)malloc(sizeof(Number)*3*n);
	
	status = IpoptSolve(myprob, x, NULL, &obj, NULL, multxl, multxu,data);
	
	
	if (status == Solve_Succeeded) {
    printf("\n\nSolution of the primal variables, x\n");
    for (i=0; i<3*n; i++) 
	 {
      printf("x[%d] = %e\n", i, x[i]); 
	 }
	
	printf("\n\n Objective function final value is %e \n",obj);
	} 
	FreeIpoptProblem(myprob);
    free(x);
    free(multxl);
    free(multxu);

    return 0;
 }
 


Bool eval_f(Index n, Number* x, Bool new_x,Number* obj_val, UserDataPtr data)

 { 
  Number a,b,c;
  Udata *user_data=(Udata *)data;
  Index h;
  h=n/3;
  
  a=user_data->a;
  b=user_data->b;
  c=user_data->c;
  
  *obj_val=-((x[3*(h/2)]*a)+(x[(3*(h/2))+1]*b)+(x[(3*(h/2))+2]*c)); //evaluating Objective Function
  return TRUE; 
 }

Bool eval_grad_f(Index n, Number* x, Bool new_x,Number* grad_f, UserDataPtr data)
 {
  Index i,h;
  Udata *user_data=(Udata *)data;
  h=n/3;
  for(i=0;i<3*h;i++)
   {
    if(i==3*(h/2))
	 grad_f[i]=-user_data->a;
	else if(i==(3*(h/2))+1)
	 grad_f[i]=-user_data->b;
	else if(i==(3*(h/2))+2)
	 grad_f[i]=-user_data->c;
	else
	 grad_f[i]=0;
   }
   return TRUE;
 }

Bool eval_g(Index n, Number* x, Bool new_x,Index m, Number* g, UserDataPtr data)
  {
   Index i,h;
   
   Udata *user_data=(Udata *)data;
   h=n/3;
   assert(m==2*h+3);
   /*printf("n is %d ",n);*/
   
   /*for(i=0;i<3*n;i++)
	 {
	  printf("\n values before are %f    \n",x[i]);
	 }*/
    g[0]=square(x[0]-user_data->StartVert[1][0])+square(x[1]-user_data->StartVert[1][1])+square(x[2]-user_data->StartVert[1][2]);
   
   g[1]=square(user_data->EndVert[0][0]-x[3*h-3])+square(user_data->EndVert[0][1]-x[3*h-2])+square(user_data->EndVert[0][2]-x[3*h-1]);
   
   g[2]=square(x[3]-2*x[0]+user_data->StartVert[1][0])+square(x[4]-2*x[1]+user_data->StartVert[1][1])+square(x[5]-2*x[2]+user_data->StartVert[1][2]);
   
   g[3]=square(user_data->EndVert[0][0]-2*x[3*h-3]+x[3*h-6])+square(user_data->EndVert[0][1]-2*x[3*h-2]+x[3*h-5])+square(user_data->EndVert[0][2]-2*x[3*h-1]+x[3*h-4]);
   
   g[4]=square(x[0]-2*user_data->StartVert[1][0]+user_data->StartVert[0][0])+square(x[1]-2*user_data->StartVert[1][1]+user_data->StartVert[0][1])+square(x[2]-2*user_data->StartVert[1][2]+user_data->StartVert[0][2]);
    
   g[5]=square(user_data->EndVert[1][0]-2*user_data->EndVert[0][0]+x[3*h-3])+square(user_data->EndVert[1][1]-2*user_data->EndVert[0][1]+x[3*h-2])+square(user_data->EndVert[1][2]-2*user_data->EndVert[0][2]+x[3*h-1]);
    
   for(i=0;i<h-1;i++)
    {
	 g[i+6]=square(x[3*i+3]-x[3*i])+square(x[3*i+4]-x[3*i+1])+square(x[3*i+5]-x[3*i+2]);
	}
   for(i=0;i<h-2;i++)
    {
	 g[i+(h-1)+6]=square(x[3*i+6]-2*x[3*i+3]+x[3*i])+square(x[3*i+7]-2*x[3*i+4]+x[3*i+1])+square(x[3*i+8]-2*x[3*i+5]+x[3*i+2]);
    }
  	
	
   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 data)
 {
  Index i,h;
  Index j=0;
  Udata *user_data=(Udata *)data;
 // static Number **jac;
   //static Index *r,*c;
  h=n/3;
  assert(m==2*h+3);
	 
  if(values==NULL)
   {
	for(i=0;i<3;i++)
	 {
	  iRow[i]=0;jCol[i]=i;
	 }
	for(i=0;i<3;i++)
	 {
	  iRow[i+3]=1;jCol[i+3]=(3*h-(3-i));
	 }  
	for(i=0;i<6;i++)
	 {
	  iRow[i+6]=2;jCol[i+6]=i;
	 }
	for(i=0;i<6;i++)
	 {
	  iRow[i+12]=3;jCol[i+12]=3*h-(6-i);
	 }
	for(i=0;i<3;i++)
	 {
	  iRow[i+18]=4;jCol[i+18]=i;
	 }
	for(i=0;i<3;i++)
	 {
	  iRow[i+21]=5;jCol[i+21]=3*h-(3-i);
	 }   

	for(i=0;i<6*(h-1);i++)
	 {
	  if((i%6)==0)j=0;
	  iRow[i+24]=(i/6)+6;jCol[i+24]=3*(i/6)+j;
	  j++;
	 }
	for(i=0;i<9*(h-2);i++)
	 {
	  if((i%9)==0)j=0;
	  iRow[24+i+6*(h-1)]=(i/9)+(h-1)+6;jCol[24+i+6*(h-1)]=3*(i/9)+j;
	  j++;
	 }

		   
   }
  else
   {
    for(i=0;i<3;i++)
	 {
	  values[i]=2*(x[i]-user_data->StartVert[1][i]);
	 }
	for(i=0;i<3;i++)
	 {
	  values[i+3]=-2*(user_data->EndVert[0][i]-x[3*h-(3-i)]);
	 }
	for(i=0;i<3;i++)
	 {
	  values[i+6]=-4*(x[i+3]-2*x[i]+user_data->StartVert[1][i]);
	 }
	for(i=0;i<3;i++)
	 {
	  values[i+9]=2*(x[i+3]-2*x[i]+user_data->StartVert[1][i]);
	 }
	for(i=0;i<3;i++)
	 {
	  values[i+12]=2*(user_data->EndVert[0][i]-2*x[3*h-(3-i)]+x[3*h-(6-i)]);
	 }
	for(i=0;i<3;i++)
	 {
	  values[i+15]=-4*(user_data->EndVert[0][i]-2*x[3*h-(3-i)]+x[3*h-(6-i)]);
	 }
	 
	for(i=0;i<3;i++)
	 {
	  values[i+18]=2*(x[i]-2*user_data->StartVert[1][i]+user_data->StartVert[0][i]);
	 }
	for(i=0;i<3;i++)
	 {
	  values[i+21]=2*(user_data->EndVert[1][i]-2*user_data->EndVert[0][i]+x[3*h-(3-i)]);
	 } 

	
    for(i=0;i<(h-1);i++)
	 {
	   values[24+(6*i)]=-2*(x[3*i+3]-x[3*i]);
	   values[24+(6*i)+1]=-2*(x[3*i+4]-x[3*i+1]); 
	   values[24+(6*i)+2]=-2*(x[3*i+5]-x[3*i+2]);
	   values[24+(6*i)+3]=2*(x[3*i+3]-x[3*i]);
	   values[24+(6*i)+4]=2*(x[3*i+4]-x[3*i+1]); 
	   values[24+(6*i)+5]=2*(x[3*i+5]-x[3*i+2]);  
	 }
    for(i=0;i<h-2;i++)
	 {
	  values[24+(9*i)+(6*h-6)]=2*(x[3*i+6]-2*x[3*i+3]+x[3*i]);
	  values[24+(9*i)+(6*h-6)+1]=2*(x[3*i+7]-2*x[3*i+4]+x[3*i+1]);
	  values[24+(9*i)+(6*h-6)+2]=2*(x[3*i+8]-2*x[3*i+5]+x[3*i+2]);
	  values[24+(9*i)+(6*h-6)+3]=-4*(x[3*i+6]-2*x[3*i+3]+x[3*i]);
	  values[24+(9*i)+(6*h-6)+4]=-4*(x[3*i+7]-2*x[3*i+4]+x[3*i+1]);
	  values[24+(9*i)+(6*h-6)+5]=-4*(x[3*i+8]-2*x[3*i+5]+x[3*i+2]);
	  values[24+(9*i)+(6*h-6)+6]=2*(x[3*i+6]-2*x[3*i+3]+x[3*i]);
	  values[24+(9*i)+(6*h-6)+7]=2*(x[3*i+7]-2*x[3*i+4]+x[3*i+1]);
	  values[24+(9*i)+(6*h-6)+8]=2*(x[3*i+8]-2*x[3*i+5]+x[3*i+2]);
	 }
		
	 /*
	 for(i=0;i<15*n-6;i++)
	    {
		 printf("\n %d value for jacobians is  %f ",i+1,values[i]);
		} 
      */
	   }
   
   
   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 data)
 {
  Index i,j,h;
  /*static Number **hess;
  static Index *r,*c;*/
  /*Udata *user_data=(Udata *)data;*/
  
   h=n/3;
  assert(m==2*h+3);

  if(values==NULL)
   { 
    
	for(i=0;i<3*h;i++)
	 {
	  iRow[i]=i;jCol[i]=i;
	 }
	for(i=0;i<3*h-3;i++)
	 {
	  iRow[i+(3*h)]=3+i;jCol[i+(3*h)]=i;
	 }
	for(i=0;i<3*h-6;i++)
	 {
	  iRow[i+(6*h-3)]=6+i;jCol[i+(6*h-3)]=i;
	 }
    } 
   else
    {
	 /*for G*/
	 for(i=0;i<(h-1);i++)
	 {
      for(j=0;j<6;j++)
	   values[3*i+j]+=2*lambda[i+6];
	   
	  for(j=0;j<3;j++)
	   values[(3*h)+(3*i)+j]+=-2*lambda[i+6];
	 }
	 
	 /*for S*/
	 for(i=0;i<h-2;i++)
	  {
	   values[3*i]+=2*lambda[i+(h-1)+6];
	   values[3*i+1]+=2*lambda[i+(h-1)+6];
	   values[3*i+2]+=2*lambda[i+(h-1)+6];
	   values[3*i+3]+=8*lambda[i+(h-1)+6];
	   values[3*i+4]+=8*lambda[i+(h-1)+6];
	   values[3*i+5]+=8*lambda[i+(h-1)+6];
	   values[3*i+6]+=2*lambda[i+(h-1)+6];
	   values[3*i+7]+=2*lambda[i+(h-1)+6];
	   values[3*i+8]+=2*lambda[i+(h-1)+6];
	   
	   for(j=0;j<6;j++)
	   values[(3*h)+(3*i)+j]+=-4*lambda[i+(h-1)+6];
	   
	   for(j=0;j<3;j++)
	   values[(6*h-3)+(3*i)+j]+=2*lambda[i+(h-1)+6];
	  }
	   
	 /*for other constraints */  
	   values[0]+=2*lambda[0];
	   values[1]+=2*lambda[0];
	   values[2]+=2*lambda[0];
	   
	   values[3*h-3]+=2*lambda[1];
	   values[3*h-2]+=2*lambda[1];
	   values[3*h-1]+=2*lambda[1];
	   
	   values[0]+=8*lambda[2];
	   values[1]+=8*lambda[2];
	   values[2]+=8*lambda[2];
	   values[3]+=2*lambda[2];
	   values[4]+=2*lambda[2];
	   values[5]+=2*lambda[2];
	   values[3*h]+=-4*lambda[2];
	   values[3*h+1]+=-4*lambda[2];
	   values[3*h+2]+=-4*lambda[2];
	   
	   values[3*h-6]+=2*lambda[3];
	   values[3*h-5]+=2*lambda[3];
	   values[3*h-4]+=2*lambda[3];
	   values[3*h-3]+=8*lambda[3];
	   values[3*h-2]+=8*lambda[3];
	   values[3*h-1]+=8*lambda[3];
	   values[6*h-6]+=-4*lambda[3];
	   values[6*h-5]+=-4*lambda[3];
	   values[6*h-4]+=-4*lambda[3];
	   
	   
	   values[0]+=2*lambda[4];
	   values[1]+=2*lambda[4];
	   values[2]+=2*lambda[4];
	   
	   values[3*h-3]+=2*lambda[5];
	   values[3*h-2]+=2*lambda[5];
	   values[3*h-1]+=2*lambda[5];
	
			
	   
	}
  return TRUE;
 }