#!/usr/bin/env python3

import sys
import numpy as np
import json

if __name__ == '__main__':
    if len(sys.argv) != 2:
        sys.exit("Usage: robot input_file")
    json_input = sys.argv[1]
    with open(json_input, 'r') as fd:
        inputs = json.load(fd)
    theta1 = inputs['theta1'] * np.pi * 2
    theta2 = inputs['theta2'] * np.pi * 2
    theta3 = inputs['theta3'] * np.pi * 2
    theta4 = inputs['theta4'] * np.pi * 2
    L1 = inputs['L1']
    L2 = inputs['L2']
    L3 = inputs['L3']
    L4 = inputs['L4']
    outfile = inputs['outfile']
    u = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2) +\
        L3 * np.cos(theta1 + theta2 + theta3) +\
        L4 * np.cos(theta1 + theta2 + theta3 + theta4)
    v = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) +\
        L3 * np.sin(theta1 + theta2 + theta3) +\
        L4 * np.sin(theta1 + theta2 + theta3 + theta4)
    y = np.sqrt(u * u + v * v)
    with open(outfile, 'w') as fd:
        json.dump({'distance' : y}, fd)
    
