function save_simulated_servo_data(data, fs) {
    var flight_time = [];
    var servo_angle = [];
    var servo_speed = [];
    for (let i = 1; i < data.length; i++) {
        flight_time.push(Math.round(data[i].flight_time*100)/100);
        servo_angle.push(data[i].servo_angle);
        servo_speed.push(data[i].parachute_retraction_speed);
    }
    
    fs.writeFile('servo_data.json', flight_time + '\n' + servo_angle + '\n' + servo_speed, err => {
        if (err) {
          console.error(err)
          return
        }
        //file written successfully
      })
}

function send_data(data, ALTIMETER_ERROR, ACCELEROMETER_ERROR, port) {
    var msg = data.flight_time + "T ";
    msg += data.altitude*get_random_percent(ALTIMETER_ERROR) + "A ";
    msg += data.accel.x*get_random_percent(ACCELEROMETER_ERROR) + "X ";
    msg += data.accel.y*get_random_percent(ACCELEROMETER_ERROR) + "Y ";
    msg += data.accel.z*get_random_percent(ACCELEROMETER_ERROR) + "Z";
    msg += "!";

    port.write(msg);
    console.log();
    console.log("Sent: " + msg);
}

function plot_data(data, fc_data, plotly, open, fs) {
    var flight_time_values = [];


    // Graph 1 data
    var altitude_values = [];
    var accel_x_values = [];
    var accel_y_values = [];
    var accel_z_values = [];
    for (let i = 1; i < fc_data.length; i++) {
        flight_time_values.push(fc_data[i].flight_time);
        altitude_values.push(fc_data[i]["altitude"]);
        accel_x_values.push(fc_data[i]["accel"].x);
        accel_y_values.push(fc_data[i]["accel"].y);
        accel_z_values.push(fc_data[i]["accel"].z);
        
    }
    var altitude_trace = {
        x: flight_time_values,
        y: altitude_values,
        name: "Actual Altitude (m)",
        type: "scatter",
    };
    var accel_x_trace = {
        x: flight_time_values,
        y: accel_x_values,
        name: "Sensor Accel X (m/s/s)",
        type: "scatter",
    };
    var accel_y_trace = {
        x: flight_time_values,
        y: accel_y_values,
        name: "Sensor Accel Y (m/s/s)",
        type: "scatter",
    };
    var accel_z_trace = {
        x: flight_time_values,
        y: accel_z_values,
        name: "Sensor Accel Z (m/s/s)",
        type: "scatter",
    };
    var graph1_data = {
        data: [altitude_trace, accel_x_trace, accel_y_trace, accel_z_trace],
    };


    // Graph 2 data
    var predicted_apogee_values = [];
    var servo_angle_values = [];
    var drag_force_values = [];
    var vel_x_values = [];
    var vel_y_values = [];
    var vel_z_values = [];
    var pos_x_values = [];
    var pos_y_values = [];
    var pos_z_values = [];
    for (let i = 1; i < fc_data.length; i++) {
        predicted_apogee_values.push(fc_data[i]["predicted_apogee"]);
        servo_angle_values.push(fc_data[i]["servo_angle"]);
        drag_force_values.push(fc_data[i]["total_drag"]);
        vel_x_values.push(fc_data[i]["integration_velocity"].x);
        vel_y_values.push(fc_data[i]["integration_velocity"].y);
        vel_z_values.push(fc_data[i]["integration_velocity"].z);
        pos_x_values.push(fc_data[i]["integration_position"].x);
        pos_y_values.push(fc_data[i]["integration_position"].y);
        pos_z_values.push(fc_data[i]["integration_position"].z);
    }
    var predicted_apogee_trace = {
        x: flight_time_values,
        y: predicted_apogee_values,
        name: "Predicted Apogee (m)",
        type: "scatter",
    };
    var servo_angle_trace = {
        x: flight_time_values,
        y: servo_angle_values,
        name: "Servo Angle",
        type: "scatter",
    };
    var drag_force_trace = {
        x: flight_time_values,
        y: drag_force_values,
        name: "Drag Force (N)",
        type: "scatter",
    };
    var vel_x_trace = {
        x: flight_time_values,
        y: vel_x_values,
        name: "Integrated Velocity X (m/s)",
        type: "scatter",
    };
    var vel_y_trace = {
        x: flight_time_values,
        y: vel_y_values,
        name: "Integrated Velocity Y (m/s)",
        type: "scatter",
    };
    var vel_z_trace = {
        x: flight_time_values,
        y: vel_z_values,
        name: "Integrated Velocity Z (m/s)",
        type: "scatter",
    };
    var pos_x_trace = {
        x: flight_time_values,
        y: pos_x_values,
        name: "Integrated Position X (m)",
        type: "scatter",
    };
    var pos_y_trace = {
        x: flight_time_values,
        y: pos_y_values,
        name: "Integrated Position Y (m)",
        type: "scatter",
    };
    var pos_z_trace = {
        x: flight_time_values,
        y: pos_z_values,
        name: "Integrated Position Z (m)",
        type: "scatter",
    };
    var graph2_data = {
        data: [predicted_apogee_trace, altitude_trace, servo_angle_trace, drag_force_trace, vel_x_trace, vel_y_trace, vel_z_trace, pos_x_trace, pos_y_trace, pos_z_trace],
    };


    // Graph 3 data
    var predicted_time_of_flight_values = [];
    var main_parachute_retract_speed_values = [];
    var main_parachute_retract_distance_values = [];
    for (let i = 1; i < fc_data.length; i++) {
        predicted_time_of_flight_values.push(fc_data[i]["predicted_time_of_flight"]);
        main_parachute_retract_speed_values.push(fc_data[i]["main_parachute_retract_speed"]);
        main_parachute_retract_distance_values.push(fc_data[i]["main_parachute_retract_distance"]);
    }
    var predicted_time_of_flight_trace = {
        x: flight_time_values,
        y: predicted_time_of_flight_values,
        name: "Predicted Time of Flight (s)",
        type: "scatter",
    };
    var main_parachute_retract_speed_trace = {
        x: flight_time_values,
        y: main_parachute_retract_speed_values,
        name: "Main Parachute Retract Speed (cm/s)",
        type: "scatter",
    };
    var main_parachute_retract_distance_trace = {
        x: flight_time_values,
        y: main_parachute_retract_distance_values,
        name: "Main Parachute Retract Distance (cm)",
        type: "scatter",
    };
    var graph3_data = {
        //data: [predicted_time_of_flight_trace, altitude_trace, accel_x_trace, accel_y_trace, accel_z_trace, drag_force_trace, vel_x_trace, vel_y_trace, vel_z_trace, main_parachute_retract_speed_trace],
        data: [predicted_time_of_flight_trace, altitude_trace, accel_x_trace, accel_y_trace, accel_z_trace, drag_force_trace, vel_x_trace, vel_y_trace, vel_z_trace, main_parachute_retract_speed_trace, main_parachute_retract_distance_trace],
    };
      
    var imgOptions = {
        format: 'pdf',
        width: 1000,
        height: 500, 
    };
    
    // Exporting pdfs
    plotly.getImage(graph1_data, imgOptions, function (err, imgStream) {
        if (err) return console.log (err);
      
        var fileStream = fs.createWriteStream('Graph 1.pdf');
        imgStream.pipe(fileStream);
    });

    plotly.getImage(graph2_data, imgOptions, function (err, imgStream) {
        if (err) return console.log (err);
      
        var fileStream = fs.createWriteStream('Graph 2.pdf');
        imgStream.pipe(fileStream);
    });

    plotly.getImage(graph3_data, imgOptions, function (err, imgStream) {
        if (err) return console.log (err);
      
        var fileStream = fs.createWriteStream('Graph 3.pdf');
        imgStream.pipe(fileStream);
    });
    
    
    /*
    // More featured plot accessed through plotly's website
    var layout = {
        title: "Simulator Altitude",
        xaxis: {
            title: "Seconds (s)",
        },
        yaxis: {title: "Altitude (m)"},
        yaxis2: {
            title: "Predicted Altitude (m)",
            titlefont: {color: "rgb(148, 103, 189)"},
            tickfont: {color: "rgb(148, 103, 189)"},
            overlaying: "y",
            side: "right"
        },
    };
    var graphOptions = {
        layout: layout, 
        filename: "Altitude Plot", 
        fileopt: "overwrite"
    };
    plotly.plot(data.data, graphOptions, function (err, msg) {
        open(msg.url);
    });
    */
}

function calculate_drag(cd, area, velocity, air_density) {
    return 0.5 * air_density * velocity * velocity * cd * area;
}

function interpolate_data(x_data, y_data, num_x_vals, actual_x_val) {
    if (actual_x_val > x_data[num_x_vals-1]) {
      return 0;
    }
    var lower_bound_index;
    for (let i = 0; i < num_x_vals; i++) {
        if (actual_x_val == x_data[i]) {  // If the x value we're looking at lands exactly at a data point, just use it
            return y_data[i];  // Return the y value associated with the x value we are looking at
        } else if (actual_x_val > x_data[i]) {  // The x value we're looking for is greater than the value we're comparing it to, then we just passed the suitable range 
            lower_bound_index = i;  // Save the index of the last x value where we are greater than
        } else {  // If the x value we're looking for is less than the value we're comparing it to, stop
            break;
        }
    }
  
    // Now that we have the lower bound index, we know which two x values the data point will fall between, 
    // so we create a secant line between the two bounds and then grab the estimated y value
    // y = mx + b  -> y = (y2-y1)/(x2-x1)*(x-x1) + y1
    // where (x1,y1) is the lower bound data point and (x2,y2) is the upper bound data point
  
    return ((y_data[lower_bound_index+1] - y_data[lower_bound_index])/(x_data[lower_bound_index+1] - x_data[lower_bound_index]))*(actual_x_val - x_data[lower_bound_index]) + y_data[lower_bound_index];
}

function sign(x) {
    if (x == 0) {
        return 0;
    } else if (x < 0) {
        return -1;
    } else {
        return 1;
    }
}

function get_random_percent(max) {
    var rand1 = Math.random();
    if (rand1 < 0.5) {
        rand1 = -1;
    } else {
        rand1 = 1;
    }
    return 1+(Math.floor(Math.random()*max*100) * rand1)*0.0001;
}

module.exports = {save_simulated_servo_data, send_data, plot_data, calculate_drag, interpolate_data, sign, get_random_percent};