
#include <Formula.h>

/* e.g. a*{b-c}+sin(Pi)+x[1] */

enum TokenType {Tconst, Tfunction,
		  Tidentifier, TlocalVar, Tbracket, Tinvalid};
enum opType {Plus, Minus, Mult, Div, Pow, Lbrace, Rbrace, Lindex, Rindex,
	       Comma, None, Mnone, Function};
string opString("+-*/^()[], ");
const char Lb = '{', Rb = '}'; /* these define limiters for expressions */


enum fnType      {sin,cos,tan,exp,pow,log,log10,minus};
int  fnNArgs[] = { 1 , 1 , 1 , 1 , 2 , 1 , 1   , 1   };
string fnName[]= {"sin","cos","tan","exp","pow", 
		    "log","log10","minus", "empty"};

inline double fnMinus(const double &a) {return(-a);}

 Formula::Formula()
{
  if (!init)
    {
      init = true;
    };

}


int Formula::parseFunction(const string &fn)
{int i = 0;
  while (fnName[i] != "empty")
    if (fnName[i] == fn) return(i);
    else i++;

 return(-1);
}

inline double Formula::evalFunction(const fnType &fn, const double &(*d))
{
  switch(fn)
    {
    case sin: return(sin(d[]));
    case cos: return(cos(d[]));
    case tan: return(tan(d[]));
    case exp: return(exp(d[]));
    case pow: return(pow(d[],d[1]));
    case log: return(log(d[]));
    case log10: return(log10(d[]));
    case minus: return(fnMinus(d[]));
    default: Error(1,"Formula::evalFunction, Invalid fntype \n"); return(-1);
      break;
    }
}

void Formula::set(const IDLabel &id)
{
  this->id = id;
}

int Formula::addFormula(Formula *f)
/* add a formula to the global database of formulas Q? */
{
  function[f->id.label()] = (f->eval);
}

/* Code stored in code[][0,1]

   int *code, *bracket, nCode, nConsts, nBrackets;
   double *consts, *bracketVal;

   code[][0] : operator   type - Plus, Minus, Mult, Div, Pow, Comma, Rbrace,None
   code[][1] : token      type - identifier, function, const, bracket
   code[][2] : index of token      -do-       -do-     -do-  . . . 

e.g.  4*X^2 + sin[90*Pi/2.0] + exp[y]

nBrackets = 2

consts[]: 90.0, Pi, 2.0, 4.0
identifiers: x, y, ... -> Universe->get()
function[]: sin(1), cos(1), exp(1), minus(1), ...

code[][] for bracket1 - 
   const, 1
*  const, 2
/  const, 3

code[][] for bracket2 - 
   const: 4
*  ident: 1
^  const: 3
+  func : 1
]  brac : 1
+  func : 3
]  ident: 2

*/

double Formula::evaluate(double *d)
{
  register int operator, *end, *code, *brac;
  register double *(void *) fn;
  register double temp, expression, term;
  double arg[MaxArgs], *argPtr;
  
  code = this->code;
  
  for (register int b = 0, brac = bracket; b < nBrackets; b++)
    {      
      end = code + brac[]*J; /* is this & code++ valid (=> code += 3 int ) Q? */
      while (code <= end)
	{
	  operator = code[];
	  
	  switch(code[1])
	    {
	    case Tidentifier: 	      break;
	      
	    case TlocalVar: break;

	    case Tconst:
	      temp = consts[code[2]]; break;
	      
	    case Tbracket:
	      temp = bracket[code[2]]; break;
	      
	    case Tfunction:
	      argPtr = arg; 
	      int fnCode = code[2];
	      do
		{
		  code += J;
		  switch(code[1])
		    {
		    case Tidentifier: 	      break;

		    case TlocalVar: break;
		      
		    case Tconst:
		      argPtr[] = consts[code[2]]; break;
		      
		    case Tbracket:
		      argPtr[] = bracket[code[2]]; break;
		      
		    default:
		      Error(3,"Formula::evaluate, unknown identifier in " +
			    id.label() + " \n");
		    };
		  argPtr++;
		}
	      while(code[] != Rbrace);
	      temp = evalFunction(fnCode, arg);
	      break;
	      
	    default:
	      Error(2,"Formula::evaluate, unknown identifier in " +
		    id.label() + " \n");      
	    };
	  
	  switch(operator)
	    {
	    case None:
	      expression = 0.0; term = temp; break;
	      
	    case Mnone:
	      expression = 0.0; term = -temp; break;
	      
	    case Plus:
	      expression += term; term = temp; break;

	    case Minus:
	      expression += term; term = -temp; break;
	      
	    case Mult:
	      term *= temp; break;
	    case Div:
	      term /= temp; break;
	      
	    default:
	      Error(1,"Formula::evaluate, Unknown operator in " +
		    id.label() + " \n");
	    };
	  
	  code += J;
	};
      *brac = expression;
      brac++;
    };
  
  return (expression);
}

int In (char c, const string &s)
{ return(s.pos(c)); }

void stripBlanks(string *s) 
     /* removes all blanks */
{ int i = 0;

  while (i < s->length())
      if (*s[i] == ' ') s->del(i)
      else 
	{
	  /* Convert to Lower Case */
	  /* note in ascii, 'a'(141) > 'A'(101), important */
	  if ((s[i] >= 'A') && (s[i] <= 'Z')) s[i] += ('a' - 'A');
	  i++;
	};
}

int Formula::extractBrackets(string *h, string *s, )
{int p1,p2, bcount = 0;
 const bufLen = 128;
 char buf[sLen];

 p1 = In (Lb, *s);
 if (p1 == -1)
   { *h = ""; return(p1); }

 p2 = p1;
 while ((p2 < s->length()) && (bcount <= 0))
   {
     p2++;
     if      (s[p2] == Rb) bcount++
     else if (s[p2] == Lb) bcount--;
   };

 if (bcount <= 0)
   {
     Error(1,"Formula::extractBrackets, bad string:"+*s+": \n");
     return(-2);
   }
 else
   {
     *h = *s(p1+1,p2-1); /* extract the string between () */
     /* insert function bracket w id number in place of original
	expression inside the Brackets () */
     ostrstream(buf, bufLen) << nBrackets;
     s->insert(p1,p2,"bracket[" + buf + "]");
   };
}

int Formula::getConst(const string &h, bool index=false)
{
  bufLen = 128; char buf[bufLen];
  ostrstream(buf, bufLen) << h;
  double d;
  istrstream(buf, bufLen) >> d;

  if (index) return (rint(d));

  int i=0;
  while(i < nConsts)
    {
      if (consts[i] == d) return (i);
    };
  consts[nConsts] = d;
  nConsts++;
  return(nConsts-1);
}

bool Formula::decode(string s)
{
  int code[MaxExpressionLen][J];
  double consts[MaxExpressionLen];
  int bracket[MaxNest];

  this->~Formula(); /* free all memory so it can be reassigned */
  this->code = code;
  this->consts = consts;
  this->bracket = bracket;
  /* Initialize code & constants arrays */
  nCode = 0; nConsts = 0; nBrackets = 0;

  /* Parse string */
  stripBlanks(&s);
  if (s[0] != '(') s = '(' + s + ')';
  this->parse(&s);
  
  /* copy code & const arrays into new arrays of exact size */
  /* use memcpy Q? or is there something more effecient */

  int i,j;
  int *tcode = new int[nCode][J];
  for (i=0; i < nCode; i++) 
    for (j=0; j < J; j++) tcode[i][j] = code[i][j];
  
  int *tbracket = new int[nBrackets];
  this->bracketVal = new double[nBrackets];
  for (i=0; i < nBrackets; i++)
    tbracket[i] = bracket[i];

  double *tconsts = new double[nConsts];
  for (i=0; i < nCode; i++) tconsts[i] = consts[i];

  /* Set formula pointers to the new arrays, as the old ones
     will be destroyed once this function returns */
  this->code = tcode; this->consts = tconsts;
  this->bracket = tbracket;

  return (true);
}


int Formula::parse(string *s)
{
  string h, fn;
  opType op, oldOp = None;
  TokenType tokenType;
  int nEntries = 0, nargsFn; 

  do
    {
      /* Left to Right, Scan for any braces, if found, extract string
	 and call parse, else parse the () free algebraic expression */
      extractBrackets(&h, s);
      int tempNb = nBrackets;
      if (h->length() > 0) 
	{
	  bracket[tempNb] = parse(&h); 
	  nBrackets++;
	};
    }
  while (h->length() > 0);

  while (getToken(&h, &op, s, &tokenType))
    {

      if (h.length() == 0)
	{
	  if (((op == Minus) || (op == Plus)) && (oldOp == None) &&
	      (nEntries == 0))
	    {if (op == Minus) oldOp = Mnone;
	     /* else if (op == Plus) -> Do exactly nothing */ }
	  else
	    Error(2,"Formula::parse, Bad invalid/empty head:"+
		  h+": tail "+*s+": \n");
	  getToken(&h, &op, s, &tokenType);
	};

      /* check if head is a number, function, symbol - else invalid */
      /* if head is a number, convert to constant & store */
      if (tokenType == Tconst)
	code[nCode][2] = getConst(h);
      
      else if (op == Lindex) 
	{
	  if (h == "bracket") 
	    tokenType = Tbracket;
	  else
	    Error(6,"bad [] " + h +" \n");
	  
	  /* parse all the way to e.g. "bracket[2]+" */
	  TokenType temp;
	  getToken(&h, &op, s, &temp);
	  if ((tokenType == Tconst) && (op == Rindex))
	    code[nCode][2] = getConst(h, true);
	  getToken(&h, &op, s, &temp);
	}
      else if((int fncode = parseFunction(h)) >= 0)
	{ tokenType = Tfunction; code[nCode][2] = fncode;
	  nargsFn = fnNArgs[fncode]; 
	  if (nargsFn > MaxArgs)
	    Error(5,"Formula::parse, Too many arguments in function:" + 
		  fnName[fncode] + ": \n");
	}
      else 
	Error(3,"Formula::parse, Bad token symbol name:"
	      + h+ ": \n");

      

      code[nCode][1] = tokenType;

      switch (op)
	{
	case Plus: 
	case Minus: 
	case Mult: 
	case Div: 
	case None: case Mnone:
	case Lbrace:
	  int narg = 0;
	  break;
	case Rbrace: 
	  /* Add argument to argument list, end of list */
	  narg++;
	  getToken(&h, &op, s, &tokenType); oldOp = Rbrace;
	  if ((nargs != nargsFn) || (h.length() > 0))
	    Error(4,"Formula::parse, no of function args do not match:"
		  + h ": \n");
	  break;
	case Comma:
	  /* Add argument to argument list */
	  narg++;
	  break;
	default: Error (1,"Invalid Op type, Formula::parse \n");
	};
      code[nCode][0] = oldOp; 
      nEntries++; nCode++;
      oldOp = op;
    };
 return(nEntries);
}


bool Formula::getToken (string *h, opType *op, string *s, 
			TokenType *tokenType) 
     /* s contains data */
{
  int index= -1, temp; 
  string numbers("0123456789");
  *tokenType = Tinvalid;

  if (s->length() == 0) 
    {
      *h = ""; 
      *op = None; return(true);
    }

  if (In(*s[0], numbers) >= 0)
    {
      *tokenType = Tconst;
      index =0;
      while (In(*s[index], numbers) >= 0)
	{
	  index ++;
	  if (In(*s[index], "eE") >= 0)
	    {
	      index++; if (In(*s[index], "+-") >= 0);
	      index++;
	    };
	};
      
      if ((temp = In(*s[0], opString)) >= 0)
	{
	  *op = opType(temp);
	  s->del(0); /* delete the operator character */
	  return (true);
	}
    }
  else
    {
      /* Scan string *s for first operator */
      index = 0;
      while ((index < s->length()) && 
	     (In(*s[index],opString) == -1))
	index++;
    };
  
  /* -------------------------------------------------------*/
  
  /* if no op found, return None w blank tail, 
     - else return operator type w head & tail */
  if (index >= s->length())
    {
      *op = None;
      *h = *s; *s = "";
    }
  else if ((temp = In(*s[index], opString)) >= 0)
    {
      *op = opType(temp);
      *h = *s(0,index-1); stripBlanks(h);
      *s = *s(index+1,s->length()-1); stripBlanks(s);
      /* stripping *s is probably redundant, but who cares */
    }
  else
    { 
      Error(1,"Formula::getToken, Invalid string:" + 
	    *s + ": \n");
      return(false); 
    };
  
  /* identify head if not a Tconst  Q? */
  
  return(true);
}
