#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#include "levmar-2.6/levmar.h"

#include "stack.h"

#define STACK_BUF_LEN 128

typedef struct {

double data[STACK_BUF_LEN];

int pos;

} D_Stack;

typedef struct {

int data[STACK_BUF_LEN];

int pos;

} I_Stack;

// typedef struct {

// double *data;

// int pos;

// } D_StackD;

// typedef struct {

// int *data;

// int pos;

// } I_StackD;

I_Stack* new_istack();

int push_istack(I_Stack* stack, int i);

void pop_istack(I_Stack* stack);

int top_istack(I_Stack* stack);

int len_istack(I_Stack* stack);

int get_istack(I_Stack* stack, int pos);

int is_empty_istack(I_Stack* stack);

void clear_istack(I_Stack* stack);

void free_istack(I_Stack* stack);

D_Stack* new_dstack();

int push_dstack(D_Stack* stack, double f);

void pop_dstack(D_Stack* stack);

double top_dstack(D_Stack* stack);

int len_dstack(D_Stack* stack);

double get_dstack(D_Stack* stack, int pos);

int is_empty_dstack(D_Stack* stack);

void clear_dstack(D_Stack* stack);

void free_dstack(D_Stack* stack);

I_Stack* new_istack() {

I_Stack* stack = (I_Stack*) malloc(sizeof(I_Stack));

stack->pos = -1;

return stack;

}

int push_istack(I_Stack* stack, int i) {

if (stack->pos +1 >= STACK_BUF_LEN)

return 0;

stack->pos++;

stack->data[stack->pos] = i;

return 1;

}

void pop_istack(I_Stack* stack) {

if (stack->pos >= 0) stack->pos--;

}

int top_istack(I_Stack* stack) {

return stack->data[stack->pos];

}

int len_istack(I_Stack* stack) {

return stack->pos + 1;

}

int get_istack(I_Stack* stack, int pos) {

return stack->data[pos];

}

int is_empty_istack(I_Stack* stack) {

return (stack->pos < 0) ? 1 : 0;

}

void clear_istack(I_Stack* stack) {

stack->pos = -1;

}

void free_istack(I_Stack* stack) {

free(stack);

}

D_Stack* new_dstack() {

D_Stack* stack = (D_Stack*) malloc(sizeof(D_Stack));

stack->pos = -1;

return stack;

}

int push_dstack(D_Stack* stack, double f) {

if (stack->pos +1 >= STACK_BUF_LEN)

abort();

// return 0;

stack->pos++;

stack->data[stack->pos] = f;

return 1;

}

void pop_dstack(D_Stack* stack) {

if (stack->pos >= 0) stack->pos--;

}

double top_dstack(D_Stack* stack) {

return stack->data[stack->pos];

}

int len_dstack(D_Stack* stack) {

return stack->pos + 1;

}

double get_dstack(D_Stack* stack, int pos) {

return stack->data[pos];

}

int is_empty_dstack(D_Stack* stack) {

return (stack->pos < 0) ? 1 : 0;

}

void clear_dstack(D_Stack* stack) {

stack->pos = -1;

}

void free_dstack(D_Stack* stack) {

free(stack);

}

double stack_eval(double t, double *c_in, double *x_in, D_Stack *d_stack, StackExpr expr );

void stack_func( double *p, double *x, int m, int n, void *data) {

StackData *sdata = (StackData*)data;

int x_dim = sdata->x_dim;

double* x_data = sdata->x_data;

D_Stack d_stack;

// d_stack.clear();

// printf( "HELLO from stack_func\n");

int i;

for( i=0; i < n; i++ ) {

// printf( "%d %d %d\n",i,x_dim,sdata->x_len);

x[i] = stack_eval(0,p,&(x_data[i*x_dim]),&d_stack,sdata->expr);

}

}

void stack_jacfunc( double *p, double *jac, int m, int n, void *data) {

StackData *sdata = (StackData*)data;

int x_dim = sdata->x_dim;

double* x_data = sdata->x_data;

D_Stack d_stack;

// d_stack.clear();

int i,j;

for( i=0; i < n; i++ ) {

for( j=0; j < m; j++ ) {

jac[i*m+j] = stack_eval(0,p,&(x_data[i*x_dim]),&d_stack,sdata->derivs[j]);

}

}

}

void stack_levmar_der( double* ygiven, double* p, const int m, const int n, void* data ) {

double opts[LM_OPTS_SZ], info[LM_INFO_SZ];

// optimization control parameters; passing to levmar NULL instead of opts reverts to defaults

opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;

opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used

// invoke the optimization function

dlevmar_der(stack_func, stack_jacfunc, p, ygiven, m, n, 1000, opts, info, NULL, NULL, data); // with analytic Jacobian

}

void stack_levmar_dif( double* ygiven, double* p, const int m, const int n, void* data ) {

double opts[LM_OPTS_SZ], info[LM_INFO_SZ];

// printf( "HELLO from stack_levmar_dif\n");

StackData *sdata = (StackData*)data;

int x_dim = sdata->x_dim;

int x_len = sdata->x_len;

// printf( "x_len: %d x_dim: %d\n",x_len,x_dim);

// optimization control parameters; passing to levmar NULL instead of opts reverts to defaults

opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;

opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used

// invoke the optimization function

dlevmar_dif(stack_func, p, ygiven, m, n, 1000, opts, info, NULL, NULL, data); // without Jacobian

}

/*

ExprTypes:

---------------

NULL: 0

STARTLEAF: 1

CONSTANT: 2

TIME: 4

SYSTEM: 5

VAR: 6

LASTLEAF: 7

STARTFUNC: 8

NEG: 9

ABS: 10

SQRT: 11

SIN: 12

COS: 13

TAN: 14

EXP: 15

LASTFUNC: 17

POWI: 18

POWF: 19

POWE: 20

DIV: 21

ADD: 22

MUL: 23

EXPR_MAX: 24

STARTVAR: 25

*/

// #define print_stack_eval 1

double stack_eval(double t, double *c_in, double *x_in, D_Stack *d_stack, StackExpr expr ) {

// I_Stack *serial = new_istack();

// I_Stack istack;

// I_Stack *serial = &istack;

clear_dstack(d_stack);

int* serial = expr.serial;

#ifdef print_stack_eval

printf("Serial: |%d|", expr.s_len);

#endif

int s;

for( s=0; s < expr.s_len; s++ ) {

// #ifdef print_stack_eval

// printf( "%d ", expr.serial[s]);

// #endif

// push_istack(serial,expr.serial[expr.s_len-s-1]);

// }

// #ifdef print_stack_eval

// printf("\n");

// #endif

// clear_dstack(d_stack);

// // fill i_stack with cmds and d_stack with leaves

// while ( !is_empty_istack(serial) ) {

// // printf( "processing serial\n");

// int val = top_istack(serial);

// pop_istack(serial);

int val = serial[s];

#ifdef print_stack_eval

int dlen = len_dstack(d_stack);

int slen = len_istack(serial);

printf( "S: %d val: %d \n", slen, val );

printf( "serial(%d): [ ", slen);

for( i=0; i < slen; i++ )

printf( "%d ", get_istack(serial,i) );

printf(" ]\n");

printf( "d_stack(%d): [ ", dlen);

for( i=0; i < dlen; i++ )

printf( "%.2f ", get_dstack(d_stack,i) );

printf(" ]\n");

#endif

switch (val) {

// CONSTANT: 2

case 2: {

s++;

int p = serial[s];

// int p = top_istack(serial);

// pop_istack(serial);

push_dstack(d_stack,c_in[p]);

}

break;

// HACK***

// CONSTANTF: 3

case 3: {

s++;

int p = serial[s];

// int p = top_istack(serial);

// pop_istack(serial);

push_dstack(d_stack,p);

}

break;

// TIME: 4

case 4:

push_dstack(d_stack,t);

break;

// SYSTEM: 5

// case 5:

// s++;

// push_dstack(d_stack,sys_in[serial[s]]);

// VAR: 6 should already be transformed, but just in case

case 6: {

s++;

int p = serial[s];

// int p = top_istack(serial);

// pop_istack(serial);

push_dstack(d_stack,x_in[p]);

break;

}

// NEG: 9

case 9: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, -top);

}

break;

// ABS: 10

case 10: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, fabs(top));

}

break;

// SQRT: 11

case 11: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, sqrt(top));

}

break;

// SIN: 12

case 12: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, sin(top));

}

break;

// COS: 13

case 13: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, cos(top));

}

break;

// TAN: 14

case 14: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, tan(top));

}

break;

// EXP: 15

case 15: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, exp(top));

}

break;

// LOG: 16

case 16: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, log(top));

}

break;

// POWI: 18

case 18: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

s++;

int pwr = serial[s];

// int pwr = top_istack(serial);

// pop_istack(serial);

push_dstack(d_stack, pow(top,pwr));

}

break;

// POWE: 20

case 20: {

double top = top_dstack(d_stack);

pop_dstack(d_stack);

double pwr = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, pow(top,pwr));

}

break;

// DIV: 21

case 21: {

double denom = top_dstack(d_stack);

pop_dstack(d_stack);

double numer = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, numer / denom);

}

break;

// ADD: 22

case 22: {

double lhs = top_dstack(d_stack);

pop_dstack(d_stack);

double rhs = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, lhs + rhs);

// printf( "%f + %f = %f\n", lhs, rhs, top_dstack(d_stack));

}

break;

// MUL: 23

case 23: {

double lhs = top_dstack(d_stack);

pop_dstack(d_stack);

double rhs = top_dstack(d_stack);

pop_dstack(d_stack);

push_dstack(d_stack, lhs * rhs);

// printf( "%f * %f = %f\n", lhs, rhs, top_dstack(d_stack));

}

break;

case 0:

default:

// STARTVAR: 25

if (val >= 25) {

push_dstack(d_stack,x_in[val-25]); // x_dim_of_var = val - STARTVAR

} else {

printf("pushing unknown cmd %d\n", val);

int s;

for( s=0; s < expr.s_len; s++ ) {

printf( "%d ", expr.serial[s]);

}

printf( "\n");

abort();

}

}

#ifdef print_stack_eval

dlen = len_dstack(d_stack);

printf( "S: %d val: %d\n", s, val);

printf( "d_stack(%d): [ ", dlen);

for( i=0; i < dlen; i++ )

printf( "%.2f ", get_dstack(d_stack,i) );

printf(" ]\n\n");

#endif

}

// free(serial);

return top_dstack(d_stack);

}