# This script converts maxima results to libfixmath function calls.

import sys
import re

def parse_matrix(m):
    rows = re.findall("\[([^\[]*)\]", m)
    return [[a.strip() for a in r.strip('[]').split(',')] for r in rows]

def convert_equation(e):
    e = re.sub(r"sin\((.)\)", r"sin_\1", e)
    e = re.sub(r"cos\((.)\)", r"cos_\1", e)
    
    e = re.sub(r"([^ ,\*+-]+)\^2", r"(\1*\1)", e)
    
    # Handle parentheses
    subgroups = {}
    def substitute(match):
        letter = chr(ord('A') + len(subgroups))
        subgroups[letter] = convert_equation(match.group(1))
        return letter
    
    #print "Before", e
    while '(' in e:
        e = re.sub(r"\(([^\(\)]*)\)", substitute, e)
    
    #print "After", e, subgroups
    
    while '*' in e:
        e = re.sub(r"([0-9]+)\s*\*\s*([^,+-]*[^ ,+-])", r"\1 # \2", e)
        e = re.sub(r"([^,+-]*[^ ,+-])\s*\*\s*([0-9]+)", r"\1 # \2", e)
        e = re.sub(r"([^ ,+-][^,+-]*)\s*\*\s*([^,+-]*[^ ,+-])", r"fix16_mul(\1, \2)", e)
    
    replaces = subgroups.items()
    replaces.reverse()
    for letter, content in replaces:
        e = e.replace(letter, '(' + content + ')')
    
    e = re.sub(r"([^t][\(+-]|^)\s*([0-9]+)\s*([\)+-]|$)", r"\1fix16_from_int(\2)\3", e)
    
    e = e.replace('#', '*')
    e = re.sub(r"([^\( ])([+-])([^ ])", r"\1 \2 \3", e)
    
    return e
    
data = sys.stdin.read().replace('\n', ' ')

meas = re.findall(r" meas\s*:[^%]*\(%o..?\)([^%]*)\(%...?\)", data)[0]
jacob = re.findall(r" jacob\s*:[^%]*\(%o..?\)([^%]*)\(%...?\)", data)[0]

print "/* Generated code, see convert_model.py */"
print

row_labels = ["MAG_X", "MAG_Y", "MAG_Z", "ACC_X", "ACC_Y", "ACC_Z"]
for i, row in enumerate(parse_matrix(meas)):
    print "\n// " + row[0]
    print "#define KALMAN_Y_%s (%s)" % (row_labels[i], convert_equation(row[0]))

for i, row in enumerate(parse_matrix(jacob)):
    for j, eq in enumerate(row):
        print "\n// " + eq
        print "#define KALMAN_H_%s_%d (%s)" % (row_labels[i], j, convert_equation(eq))