#! /usr/bin/env python

"""
Generate an RCbencemark script that outputs a sin wave
"""

from __future__ import print_function
import argparse
import math


js_template = '''/* //////////////// generated by ulog2rcbm ////////////////
- filePrefix
- data -- [[timestamp_in_ms, pwm_output], [..., ...], ...]
- samplesAvg
- rampup
///////////// User defined variables //////////// */

const filePrefix = {};
const data = {};
const samplesAvg = {};
const rampup = {};

let lastTimestamp = data[0][0];
let position = 0; // index of data array
let timeDifference;

///////////////// Beginning of the script //////////////////

if (!data) {{
    rcb.console.print("Error: data is not an array of length > 0");
    rcb.endScript();
}}

rcb.console.setVerbose(false);

rcb.files.newLogFile({{prefix: filePrefix}});

readSensor();

function readSensor() {{
    rcb.sensors.read(readDone, samplesAvg);
}}
function readDone(result) {{
    rcb.files.newLogEntry(result, readSensor);
}}

rcb.console.print("Starting...");

if (rampup) {{
    rcb.console.print("Ramping Up...");
    rcb.output.ramp("esc", 1000, data[0][1], lastTimestamp > 5 ? lastTimestamp : 5, callback); // ramp up to data[0][1] in data[0][0] or 5 sec, whichever is larger
}} else {{
    if (lastTimestamp > 0) {{
        rcb.wait(callback, lastTimestamp);
    }} else {{
        callback();
    }}
}}

function callback() {{
    rcb.output.set("esc", data[position][1]);
    position++;
    if (data.length > position) {{
        timeDifference = data[position][0] - lastTimestamp;
        lastTimestamp = data[position][0];
        rcb.wait(callback, timeDifference);
    }} else {{
        rcb.wait(function() {{
            rcb.endScript();
        }}, 1);
    }}
}}
'''

def main():
    parser = argparse.ArgumentParser(description='Generate a RCbenchmark script that produces sinusoidal wave')
    parser.add_argument('-o', '--offset', dest='offset', action='store',
                        help="offset of the wave ('1000' - '2000', default is '1500')", default='1500')
    parser.add_argument('-a', '--amplitude', dest='amplitude', action='store',
                        help="amplitude, the peak deviation of the wave from offset ('0' - '500', default is '500')", default='500')
    parser.add_argument('-f', '--frequency', dest='frequency', action='store',
                        help="angular frequency in radians per second (default is '1')", default='1')
    parser.add_argument('-p', '--phase', dest='phase', action='store',
                        help="phase, specifies in radians where in its cycle the oscillation is at t=0 (default is '0')", default='0')
    parser.add_argument('-r', '--rampup', dest='rampup', action='store_true',
                        help="ramp up from 1000 to initial value (disabled by default)", default=False)
    parser.add_argument('--avg', dest='average', action='store',
                        help="sample average (default is '1')", default='1')
    args = parser.parse_args()
    gen_script(args.offset, args.amplitude, args.frequency, args.phase, args.rampup, args.average)

def gen_script(offset, amplitude, omega, phi, rampup, sample_average):
    offset = int(float(offset))
    assert offset >= 1000 and offset <= 2000
    amplitude = int(float(amplitude))
    assert amplitude >= 0 and amplitude <= 500
    omega = float(omega)
    phi = float(phi)
    sample_average = int(sample_average)
    output_file_prefix = 'sin_wave_{}_{}_{}_{}_{}'.format(offset, amplitude, omega, phi, sample_average)
    data = [ [ timestamp, offset + amplitude * math.sin( timestamp / 1000 * omega + phi ) ] for timestamp in range(0, 1000 * 40, 25) ] # ms
    
    with open(output_file_prefix + '.js', 'w') as js_file:
        data_string = '[{}]'.format(
            ','.join(
                [ '[{},{}]'.format(row[0] / 1000, int(row[1])) for row in data ]
            )
        )
        js_file.write(js_template.format('"{}"'.format(output_file_prefix), data_string, sample_average, 'true' if rampup else 'false'))
    return output_file_prefix + '.js'

if __name__ == '__main__':
    main()
