#!/usr/bin/env python
'''
convert fixed wing PIDs in a parameter file from old system to ACPID system
'''

import os
import re

found_line = False
changed_param = False

def process_file(fname):
    global found_line
    global changed_param

    found_line = False
    changed_param = False

    print("Processing %s" % fname)
    lines = open(fname).readlines()
    for i in range(len(lines)):
        lines[i] = lines[i].rstrip()
    while lines[len(lines)-1].strip() == "":
        lines = lines[:-1]

    def find_line(pname):
        global found_line
        for i in range(len(lines)):
            if lines[i].startswith(pname) and not lines[i][len(pname)].isalpha():
                found_line = True
                return i
        return None

    def find_param(pname, dvalue):
        i = find_line(pname)
        if i is None:
            return dvalue
        s = lines[i].replace(","," ")
        a = s.split()
        return float(a[1])

    def set_param(pname, value, old_name):
        global changed_param
        i = find_line(pname)
        if i is None:
            i = find_line(old_name)
        if i is None:
            changed_param = True
            lines.append("%s %f" % (pname, value))
            return
        # maintain separator if possible
        sep = " "
        m = re.match("[A-Z_0-9]+([\s,]+)[0-9.-]+", lines[i])
        if m is not None:
            sep = m.group(1)
        changed_param = True
        lines[i] = "%s%s%f" % (pname, sep, value)

    print("Converting pitch")
    FF = find_param("PTCH2SRV_FF", 0)
    P = find_param("PTCH2SRV_P", 1.0)
    I = find_param("PTCH2SRV_I", 0.3)
    D = find_param("PTCH2SRV_D", 0.04)
    IMAX = find_param("PTCH2SRV_IMAX", 3000)
    TCONST = find_param("PTCH2SRV_TCONST", 0.5)

    if FF <= 0:
        I = max(I,0.3)

    kp_ff = max((P - I * TCONST) * TCONST  - D, 0)
    if found_line:
        set_param("PTCH_RATE_FF", FF + kp_ff, "PTCH2SRV_FF")
        set_param("PTCH_RATE_P", D, "PTCH2SRV_P")
        set_param("PTCH_RATE_I", I * TCONST, "PTCH2SRV_I")
        set_param("PTCH_RATE_D", 0, "PTCH2SRV_D")
        set_param("PTCH_RATE_IMAX", IMAX/4500.0, "PTCH2SRV_IMAX")

    found_line = False

    print("Converting roll")
    FF = find_param("RLL2SRV_FF", 0)
    P = find_param("RLL2SRV_P", 1.0)
    I = find_param("RLL2SRV_I", 0.3)
    D = find_param("RLL2SRV_D", 0.08)
    IMAX = find_param("RLL2SRV_IMAX", 3000)
    TCONST = find_param("RLL2SRV_TCONST", 0.5)

    kp_ff = max((P - I * TCONST) * TCONST  - D, 0)
    if found_line:
        set_param("RLL_RATE_FF", FF + kp_ff, "RLL2SRV_FF")
        set_param("RLL_RATE_P", D, "RLL2SRV_P")
        set_param("RLL_RATE_I", I * TCONST, "RLL2SRV_I")
        set_param("RLL_RATE_D", 0, "RLL2SRV_D")
        set_param("RLL_RATE_IMAX", IMAX/4500.0, "RLL2SRV_IMAX")

    if not changed_param:
        print("No fixed wing PID params")
        return

    print("Writing")
    tfile = fname + ".tmp"
    f = open(tfile,"w")
    for i in range(len(lines)):
        f.write("%s\n" % lines[i])
    f.close()
    os.rename(tfile, fname)

import argparse
parser = argparse.ArgumentParser(description='convert plane PIDs from old to new system')

parser.add_argument('param_file', nargs='+')
args = parser.parse_args()

for f in args.param_file:
    process_file(f)