const misc_functions = require('./Environment_Simulator_Functions.js');
const plotly = require('plotly')('judson.upchurch', 'dCbMwLXMmC8cdXua7NP1');
const open = require('open');
var fs = require('fs');

/***********************************************************************************************************************************************/
/**************************************************************Serial***************************************************************************/
/***********************************************************************************************************************************************/
const serialport = require("serialport");
const readline = require("@serialport/parser-readline");
const Plotly = require('plotly');

const port = new serialport("COM3", {baudRate: 115200});
const parser = port.pipe(new readline({delimiter: '\n'}));


const ROCKET_MASS = 630;  // Initial mass of the rocket in grams, not including the mass of the propellant

const CD_ROCKET = 0.15;  // Coefficient of drag of just the rocket without the petals or parachutes deployed
const CD_PETALS = 1.35;  // Coefficient of drag of only the petals
const CD_MAIN_PARACHUTE = 0.7;  // Coefficient of drag of only the main parachute
const CD_DROGUE_PARACHUTE = 0.5;  // Coefficient of drag of only the drogue parachute. Assumed to be lower since it trails the main parachute

// All areas are the areas perpendicular to the flow of the air in meters squared except for the streamer. Its area is its total surface area
const AREA_ROCKET = 0.013685;
// Area data for the petals as they are deployed at specific angles
// degree servo extended. divide by 3 to get petal angle
// area of single petal (mm^2)
const petal_area_data = [
    [0,15,30,45,60,75,90,105,120,135],
    [0,28.541,73.092,118.21,163.458,208.747,254.045,299.334,344.603,388.35]
];
// cm of retraction
// area of parachute (m^2)
const main_parachute_area_data = [
    [0, 10, 20, 30],
    [0.25, 0.235, 0.21, 0.175]
];
const AREA_DROGUE_PARACHUTE = 0.05;

// Data for the F52 Motor from Aerotech
const TOTAL_IMPULSE = 78;  // Newton-seconds
// time(s)
// thrust(N)
// mass remaining(g)
const F52_motor_data = [
  [0,0.012,0.033,0.056,0.097,0.115,0.13,0.153,0.168,0.182,0.206,0.238,0.258,0.314,0.39,0.428,0.501,0.565,0.688,0.749,0.837,0.901,0.971,1.088,1.144,1.173,1.222,1.275,1.339,1.389,1.42],
  [0,46.899,61.778,69.441,73.483,76.636,74.381,74.82,78.422,78.94,77.963,77.504,73.892,72.974,72.046,70.679,65.699,62.975,58.874,56.15,52.517,49.793,46.161,39.365,34.386,29.417,20.376,13.151,5.461,1.838,0],
  [36.6,36.4588,35.8863,35.1292,33.336592,32.9814,32.4131,31.5523,30.9757,30.423,29.4783,28.2303,27.4707,25.4076,22.6428,21.2822,18.7848,16.719,12.9593,11.1992,8.80035,7.15779,5.47285,2.96266,1.92661,1.46246,0.850406,0.404654,0.105843,0.0142932,0]
];

/***********************************************************************************************************************************************/
/************************************************************Simulation*************************************************************************/
/***********************************************************************************************************************************************/
// Global variable to keep track of the actual data for the rocket so the simulator can provide real data
const actual_rocket_data = {
    servo_angle: 0,  // Servo angle (deg). Divide by 3 to get petal angle
    parachute_retract_distance: 0.0,  // Main parachute retract distance (cm)
    parachute_retract_speed: 0.0,  // The speed that the parachute is retracting (cm/s)
    stage: 0,  // Stage of the rocket. 
    //0 = on launch rod, 1 = after motor ignition
    //2 = after self propelled flight, 3 = after motor burnout
    //4 = after apogee, 5 = terminal velocity
    //6 = right above the ground, 7 = after ground hit
    mass: ROCKET_MASS,  // Mass (g)
    altitude: 0.0,  // Altitude (m)
    position: {
        x: 0.0, 
        y: 0.0, 
        z: 0.0
    },
    velocity: {
        x: 0.0, 
        y: 0.0, 
        z: 0.0
    },
    accel: {
        x: 0.0, 
        y: 0.0, 
        z: 0.0
    },
    flight_time: 0.0,  // Flight time (s)
    engine_thrust: 0.0, // Engine thrust (N)
    rocket_drag: 0.0,  // Rocket drag (N)
    petal_drag: 25.0,  // Petal drag (N)
    main_parachute_drag: 0.0,  // Main parachute drag (N)
    drogue_parachute_drag: 0.0  // Drogue parachute drag (N)
};


// For sending to Arduino
var next_arduino_transmission = 0.0;
const ARDUINO_TRANSMISSION_FREQUENCY = 100;  // Times per second
const REAL_WORLD_TIME_FACTOR = 0.1;  // 1 means real time, 2 means 1 second in simulation = 2 seconds in real life, etc

// Continuously cycling loop
const dt = 0.001;  // Number of "seconds" each loop lasts, not tied to real world timing
var force_break = false;  // In case we need to stop the while loop

var last_nanoseconds = process.hrtime()[0] * 1000000000 + process.hrtime()[1];
var now_nanoseconds = process.hrtime()[0] * 1000000000 + process.hrtime()[1];

var flight_computer_ready = false;

// For Plotting
var complete_actual_flight_data = [];
var complete_flight_computer_calculated_data = [];
var next_flight_report = 0.0;
const FLIGHT_REPORT_FREQUENCY = 100;

//Environmental and simulation factors
const AIR_DENSITY = 1.225;  // kg/m^3
const ALTIMETER_ERROR = 0.5;  // Percent of error randomly applied to the calculated then sent altimeter readings
const ACCELEROMETER_ERROR = 5;  // Percent of error randomly applied to the calculated then sent accelerometer readings

const WIRE_REEL_RADIUS = 0.75;  // Radius of the wire real in cm
const MAX_SERVO_RPM = 100;  // Max rpm of the servo motor for the parachute retractor


// We have it set up so that the simulator, this code, only runs right after the arduino has replied a message back
parser.on('data', function(line) {
    console.log("Received: " + line);
    if (!flight_computer_ready) {
        if (line.trim().substring(0,1) == "S") {  // Flight computer sent an "S" so it is ready
            flight_computer_ready = true;
            port.write("S");  // Acknowledge that we have started as well
            console.log("Sent: S");
            console.log();
            return;
        }
    } // Flight computer has already said that it is ready
    if (actual_rocket_data.stage >= 1) {  // We have ignited
        // This is computing the data until the next transmission time
        while (actual_rocket_data.flight_time < next_arduino_transmission) {
            now_nanoseconds = process.hrtime()[0] * 1000000000 + process.hrtime()[1];
            // If real-time to iterate again
            if (now_nanoseconds - last_nanoseconds >= (dt*1000000000)*REAL_WORLD_TIME_FACTOR) {
                last_nanoseconds = now_nanoseconds;
                iterate_data();
                if (actual_rocket_data.stage == 7) {  // If we have reached the ground
                    port.write("DONE!");
                    console.log("Sent: DONE!");
                    misc_functions.plot_data(complete_actual_flight_data, complete_flight_computer_calculated_data, plotly, open, fs);
                    misc_functions.save_simulated_servo_data(complete_actual_flight_data, fs);
                    break;
                }
            }
        }
        // This is grabbing all of the data sent back from the Arduino in order to plot
        var rec_msg = line.trim();
        var obj2 = new Object();
        obj2 = {
            flight_time: rec_msg.substring(0, rec_msg.indexOf("A")),
            stage: rec_msg.substring(rec_msg.indexOf("A")+1, rec_msg.indexOf("B")),
            predicted_apogee: rec_msg.substring(rec_msg.indexOf("B")+1,rec_msg.indexOf("C")),
            servo_angle: rec_msg.substring(rec_msg.indexOf("C")+1,rec_msg.indexOf("D")),
            main_parachute_retract_speed: rec_msg.substring(rec_msg.indexOf("D")+1,rec_msg.indexOf("E")),
            main_parachute_retract_distance: actual_rocket_data.parachute_retract_distance,
            mass: rec_msg.substring(rec_msg.indexOf("E")+1,rec_msg.indexOf("F")),
            altitude: rec_msg.substring(rec_msg.indexOf("F")+1,rec_msg.indexOf("G")),
            integration_position: {
                x: rec_msg.substring(rec_msg.indexOf("G")+1,rec_msg.indexOf("H")),
                y: rec_msg.substring(rec_msg.indexOf("H")+1,rec_msg.indexOf("I")),
                z: rec_msg.substring(rec_msg.indexOf("I")+1,rec_msg.indexOf("J")),
            },
            integration_velocity: {
                x: rec_msg.substring(rec_msg.indexOf("J")+1,rec_msg.indexOf("K")),
                y: rec_msg.substring(rec_msg.indexOf("K")+1,rec_msg.indexOf("L")),
                z: rec_msg.substring(rec_msg.indexOf("L")+1,rec_msg.indexOf("M")),
            },
            accel: {
                x: rec_msg.substring(rec_msg.indexOf("M")+1,rec_msg.indexOf("N")),
                y: rec_msg.substring(rec_msg.indexOf("N")+1,rec_msg.indexOf("O")),
                z: rec_msg.substring(rec_msg.indexOf("O")+1,rec_msg.indexOf("P")),
            },
            total_drag: rec_msg.substring(rec_msg.indexOf("P")+1,rec_msg.indexOf("Q")),
            predicted_time_of_flight: rec_msg.substring(rec_msg.indexOf("Q")+1,rec_msg.indexOf("R")),
        };
        if (!isNaN(obj2.servo_angle)) {
            actual_rocket_data.servo_angle = obj2.servo_angle;
        }
        if (!isNaN(obj2.main_parachute_retract_speed)) {
            actual_rocket_data.parachute_retract_speed = obj2.main_parachute_retract_speed;
        }
        complete_flight_computer_calculated_data.push(obj2);
        // Sending new data
        misc_functions.send_data(actual_rocket_data, ALTIMETER_ERROR, ACCELEROMETER_ERROR, port);
        next_arduino_transmission += 1/ARDUINO_TRANSMISSION_FREQUENCY;  // Setting the next time to send data to the Arduino
    } else {  // If there has not been ignition
        if (line.trim().substring(0,1) == "I") {  // Flight computer letting us know that there was ignition
            port.write("I");  // Acknowledge that we have received ignition
            console.log("Sent: I");
            actual_rocket_data.stage = 1;  // Move up to the correct stage
        }
    }
    
});


function iterate_data() {
    // Appending the current actual flight data to an array to later plot
    if (actual_rocket_data.flight_time >= next_flight_report) {
        var obj = new Object();
        obj = {
            flight_time: next_flight_report,
            altitude: actual_rocket_data.altitude,
            velocity_x: actual_rocket_data.velocity.x,
            velocity_y: actual_rocket_data.velocity.y,
            velocity_z: actual_rocket_data.velocity.z,
            accel_x: actual_rocket_data.accel.x,
            accel_y: actual_rocket_data.accel.y,
            accel_z: actual_rocket_data.accel.z,
            stage: actual_rocket_data.stage,
            servo_angle: actual_rocket_data.servo_angle,
            parachute_retraction_distance: actual_rocket_data.parachute_retract_distance,
            parachute_retraction_speed: actual_rocket_data.parachute_retract_speed,
            mass: actual_rocket_data.mass,
            engine_thrust: actual_rocket_data.engine_thrust,
            rocket_drag: actual_rocket_data.rocket_drag,
            petal_drag: actual_rocket_data.petal_drag,
            main_parachute_drag: actual_rocket_data.main_parachute_drag,
            drogue_parachute_drag: actual_rocket_data.drogue_parachute_drag
        };
        complete_actual_flight_data.push(obj);

        next_flight_report += 1 / FLIGHT_REPORT_FREQUENCY;
    }

    // Calculate the new acceleration of the rocket
    var net_forces = 0;
    switch (actual_rocket_data.stage) {
        case 1:  // Still on the launch rod with motor ignition
            net_forces = 0;  // Redundant
            // Check to see if we are now under self-propelled flight, aka, engine thrust > weight, aka liftoff
            // Gravity
            net_forces += -0.00981*(actual_rocket_data.mass);
            // Rocket thrust
            actual_rocket_data.engine_thrust = misc_functions.interpolate_data(F52_motor_data[0], F52_motor_data[1], 31, actual_rocket_data.flight_time);
            net_forces += actual_rocket_data.engine_thrust;
            // No need to check the drag because a non-zero velocity is required before achieving drag
            // Calculate acceleration
            actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
            // Iterate the flight time
            actual_rocket_data.flight_time += dt;
            // Checking when to move to next stage
            ;
            if (actual_rocket_data.accel.z > 0.001) {  // We have liftoff
                actual_rocket_data.stage = 2;
            } else {
                //actual_rocket_data.accel.z = 0;
            }
            break;
        case 2:  // No longer supported by the launch rod, self-propelled flight, after liftoff
            // Gravity
            net_forces += -0.00981*(actual_rocket_data.mass);
            // Rocket thrust
            actual_rocket_data.engine_thrust = misc_functions.interpolate_data(F52_motor_data[0], F52_motor_data[1], 31, actual_rocket_data.flight_time);
            net_forces += actual_rocket_data.engine_thrust;
            // Rocket body drag
            actual_rocket_data.rocket_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.rocket_drag;
            // Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
            actual_rocket_data.petal_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*0.000003*misc_functions.calculate_drag(CD_PETALS, misc_functions.interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.servo_angle), actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.petal_drag;
            // Calculate acceleration
            actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
            // Iterate the flight time
            actual_rocket_data.flight_time += dt;
            // Checking when to move to next stage
            if (actual_rocket_data.flight_time >= F52_motor_data[0][30]) {  // We have motor burnout
                actual_rocket_data.stage = 3;
            }
            break;
        case 3:  // After motor burnout has occurred
            // Gravity
            net_forces += -0.00981*(actual_rocket_data.mass);
            // Rocket body drag
            actual_rocket_data.rocket_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.rocket_drag;
            // Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
            actual_rocket_data.petal_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*0.000003*misc_functions.calculate_drag(CD_PETALS, misc_functions.interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.servo_angle), actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.petal_drag;
            // Calculate acceleration
            actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
            // Iterate the flight time
            actual_rocket_data.flight_time += dt;
            // Checking when to move to next stage
            if (actual_rocket_data.accel.z <= -5) {
                if (actual_rocket_data.velocity.z >= -3 && actual_rocket_data.velocity.z <= 3) {
                    actual_rocket_data.stage = 4;
                }
            }
            break;
        case 4:  // After apogee, assume parachute release at apogee
            // Gravity
            net_forces += -0.00981*(actual_rocket_data.mass);
            // Rocket body drag
            actual_rocket_data.rocket_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.rocket_drag;
            // Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
            actual_rocket_data.petal_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*0.000003*misc_functions.calculate_drag(CD_PETALS, misc_functions.interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.servo_angle), actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.petal_drag;
            // Drogue parachute drag which should be relatively constant besides changes in velocity because it stays the same size
            actual_rocket_data.drogue_parachute_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_DROGUE_PARACHUTE, AREA_DROGUE_PARACHUTE, actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.drogue_parachute_drag;
            // Main parachute drag which changes due to its changing size
            actual_rocket_data.main_parachute_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_MAIN_PARACHUTE, misc_functions.interpolate_data(main_parachute_area_data[0], main_parachute_area_data[1], 2, actual_rocket_data.parachute_retract_distance), actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.main_parachute_drag;
            // Calculate acceleration
            actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
            // Iterate the flight time
            actual_rocket_data.flight_time += dt;
            // Checking when to move to next stage
            if (actual_rocket_data.accel.z >= -.15 && actual_rocket_data.accel.z <= 0.15) {
                actual_rocket_data.stage = 5;
            }
            break;
        case 5:  // After reaching normal terminal velocity from the parachutes
            // Gravity
            net_forces += -0.00981*(actual_rocket_data.mass);
            // Rocket body drag
            actual_rocket_data.rocket_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.rocket_drag;
            // Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
            actual_rocket_data.petal_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*0.000003*misc_functions.calculate_drag(CD_PETALS, misc_functions.interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.servo_angle), actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.petal_drag;
            // Drogue parachute drag which should be relatively constant besides changes in velocity because it stays the same size
            actual_rocket_data.drogue_parachute_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_DROGUE_PARACHUTE, AREA_DROGUE_PARACHUTE, actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.drogue_parachute_drag;
            // Main parachute drag which changes due to its changing size
            actual_rocket_data.parachute_retract_distance += 6.28318*WIRE_REEL_RADIUS*((MAX_SERVO_RPM/60.0)*(actual_rocket_data.parachute_retract_speed/180))*dt;
            //console.log(actual_rocket_data.parachute_retract_distance);
            actual_rocket_data.main_parachute_drag = -1*misc_functions.sign(actual_rocket_data.velocity.z)*misc_functions.calculate_drag(CD_MAIN_PARACHUTE, misc_functions.interpolate_data(main_parachute_area_data[0], main_parachute_area_data[1], 4, actual_rocket_data.parachute_retract_distance), actual_rocket_data.velocity.z, AIR_DENSITY);
            net_forces += actual_rocket_data.main_parachute_drag;
            //console.log(misc_functions.interpolate_data(main_parachute_area_data[0], main_parachute_area_data[1], 4, actual_rocket_data.parachute_retract_distance));
            //console.log(actual_rocket_data.main_parachute_drag);
            // Calculate acceleration
            actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
            // Iterate the flight time
            actual_rocket_data.flight_time += dt;
            // Checking when to move to next stage
            if (actual_rocket_data.altitude <= 0) {  // Within 3 meters of the ground
                //console.table(complete_actual_flight_data);
                actual_rocket_data.stage = 6;
            }
            break;
        case 6:
            if (actual_rocket_data.altitude <= 0) {
                actual_rocket_data.stage = 7;
            }
            break;
        case 7:  // After the rocket has reached the ground
            //console.table(complete_actual_flight_data);
            //console.table(complete_flight_computer_calculated_data)
            
            //misc_functions.plot_data(complete_actual_flight_data, complete_flight_computer_calculated_data, plotly, open, fs);
            
            break;
    }

    // Integrate the acceleration for a new velocity
    actual_rocket_data.velocity.z += actual_rocket_data.accel.z * dt;

    // Integrate velocity for a new altitude
    actual_rocket_data.altitude += actual_rocket_data.velocity.z * dt;
}


// Cannot exit immediately because it takes some time to make the graphs
//process.exit();