#!/usr/bin/python
#
# Generate multirotor mixer scale tables compatible with the ArduCopter layout
#

import math

print "/*"
print "* These file is automatically generated by multi_tables.py - do not edit."
print "*/"
print ""

def rcos(angleInRadians):
  return math.cos(math.radians(angleInRadians))

CCW = 1.0
CW = -CCW

quad_x = [
    [  45, CCW],
    [-135, CCW],
    [-45,  CW],
    [135,  CW],
]

quad_plus = [
    [  90, CCW],
    [ -90, CCW],
    [   0, CW],
    [ 180, CW],
]

quad_v = [
    [  18.8, 0.4242],
    [ -18.8, 1.0],
    [ -18.8, -0.4242],
    [  18.8, -1.0],
]

quad_wide = [
    [   68, CCW],
    [ -129, CCW],
    [  -68, CW],
    [  129, CW],
]

hex_x = [
    [  90, CW],
    [ -90, CCW],
    [ -30, CW],
    [ 150, CCW],
    [  30, CCW],
    [-150, CW],
]

hex_plus = [
    [   0, CW],
    [ 180, CCW],
    [-120, CW],
    [  60, CCW],
    [ -60, CCW],
    [ 120, CW],
]

hex_cox = [
    [  60, CW],
    [  60, CCW],
    [ 180, CW],
    [ 180, CCW],
    [ -60, CW],
    [ -60, CCW],
]

octa_x = [
    [  22.5, CW],
    [-157.5, CW],
    [  67.5, CCW],
    [ 157.5, CCW],
    [ -22.5, CCW],
    [-112.5, CCW],
    [ -67.5, CW],
    [ 112.5, CW],
]

octa_plus = [
    [   0, CW],
    [ 180, CW],
    [  45, CCW],
    [ 135, CCW],
    [ -45, CCW],
    [-135, CCW],
    [ -90, CW],
    [  90, CW],
]

octa_cox = [
    [  45, CCW],
    [ -45, CW],
    [-135, CCW],
    [ 135, CW],
    [ -45, CCW],
    [  45, CW],
    [ 135, CCW],
    [-135, CW],
]

twin_engine = [
    [ 90, 0.0],
    [-90, 0.0],
]

def variableName(variable):
    for variableName, value in list(globals().iteritems()):
        if value is variable:
            return variableName

tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine]


def printEnum():
    print "enum MultirotorMixer::Geometry : unsigned int {"
    for table in tables:
        print "\t{},".format(variableName(table).upper())

    print "\n\tMAX_GEOMETRY"
    print "}; // enum MultirotorMixer::Geometry\n"

def printScaleTables():
    for table in tables:
        print "const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table))
        for (angle, yawScale) in table:
            rollScale = rcos(angle + 90)
            pitchScale = rcos(angle)
            print "\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale)
        print "};\n"
    
def printScaleTablesIndex():
    print "const MultirotorMixer::Rotor *_config_index[] = {"
    for table in tables:
        print "\t&_config_{}[0],".format(variableName(table))
    print "};\n"


def printScaleTablesCounts():
    print "const unsigned _config_rotor_count[] = {"
    for table in tables:
        print "\t{}, /* {} */".format(len(table), variableName(table))
    print "};\n"
    
    

printEnum()

print "namespace {"
printScaleTables()
printScaleTablesIndex()
printScaleTablesCounts()

print "} // anonymous namespace\n"