float AP_Landing_Deepstall::update_steering()
{
    Location current_loc;
    if (!landing.ahrs.get_position(current_loc)) {
        // panic if no position source is available
        // continue the  but target just holding the wings held level as deepstall should be a minimal energy
        // configuration on the aircraft, and if a position isn't available aborting would be worse
        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: No position available. Attempting to hold level");
        memcpy(&current_loc, &landing_point, sizeof(Location));
    }
    uint32_t time = AP_HAL::millis();
    float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) / 1000.0;
    last_time = time;


    Vector2f ab = location_diff(arc_exit, extended_approach);
    ab.normalize();
    Vector2f a_air = location_diff(arc_exit, current_loc);

    crosstrack_error = a_air % ab;
    float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
    float nu1 = asinf(sine_nu1);

    if (L1_i > 0) {
        L1_xtrack_i += nu1 * L1_i / dt;
        L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
        nu1 += L1_xtrack_i;
    }

    float desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw);

    float yaw_rate = landing.ahrs.get_gyro().z;
    float yaw_rate_limit_rps = radians(yaw_rate_limit);
    float error = wrap_PI(constrain_float(desired_change / time_constant,
                                          -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);

#ifdef DEBUG_PRINTS
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
                                    (double)crosstrack_error,
                                    (double)error,
                                    (double)degrees(yaw_rate),
                                    (double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS

    return ds_PID.get_pid(error);
}

// report_compass - displays compass information.  Also called by compassmot.pde
void Copter::report_compass()
{
    cliSerial->println("Compass");
    print_divider();

    print_enabled(g.compass_enabled);

    // mag declination
    cliSerial->printf("Mag Dec: %4.4f\n",
            (double)degrees(compass.get_declination()));

    // mag offsets
    Vector3f offsets;
    for (uint8_t i=0; i<compass.get_count(); i++) {
        offsets = compass.get_offsets(i);
        // mag offsets
        cliSerial->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n",
                        (int)i,
                        (double)offsets.x,
                        (double)offsets.y,
                        (double)offsets.z);
    }

    // motor compensation
    cliSerial->print("Motor Comp: ");
    if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
        cliSerial->println("Off");
    }else{
        if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
            cliSerial->print("Throttle");
        }
        if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
            cliSerial->print("Current");
        }
        Vector3f motor_compensation;
        for (uint8_t i=0; i<compass.get_count(); i++) {
            motor_compensation = compass.get_motor_compensation(i);
            cliSerial->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n",
                        (int)i,
                        (double)motor_compensation.x,
                        (double)motor_compensation.y,
                        (double)motor_compensation.z);
        }
    }
    print_blanks(1);
}

/*************************************************************************
 * //Function to calculate the course between two waypoints
 * //I'm using the real formulas--no lookup table fakes!
 *************************************************************************/
int get_gps_course(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc; 
  }
  return bear_calc;
}

void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
{
    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);

    Vector3f wind = landing.ahrs.wind_estimate();
    // TODO: Support a user defined approach heading
    target_heading_deg = use_current_heading ? landing.ahrs.yaw_sensor * 1e-2 : (degrees(atan2f(-wind.y, -wind.x)));

    memcpy(&extended_approach, &landing_point, sizeof(Location));
    memcpy(&arc_exit, &landing_point, sizeof(Location));

    //extend the approach point to 1km away so that there is always a navigational target
    location_update(extended_approach, target_heading_deg, 1000.0);

    float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ?  landing_point.alt * 0.01f : approach_alt_offset,
                                                             false);
    float approach_extension_m = expected_travel_distance + approach_extension;
    float loiter_radius_m_abs = fabsf(loiter_radius);
    // an approach extensions must be at least half the loiter radius, or the aircraft has a
    // decent chance to be misaligned on final approach
    approach_extension_m = MAX(approach_extension_m, loiter_radius_m_abs * 0.5f);

    location_update(arc_exit, target_heading_deg + 180, approach_extension_m);
    memcpy(&arc, &arc_exit, sizeof(Location));
    memcpy(&arc_entry, &arc_exit, sizeof(Location));

    float arc_heading_deg = target_heading_deg + (landing_point.flags.loiter_ccw ? -90.0f : 90.0f);
    location_update(arc, arc_heading_deg, loiter_radius_m_abs);
    location_update(arc_entry, arc_heading_deg, loiter_radius_m_abs * 2);

#ifdef DEBUG_PRINTS
    // TODO: Send this information via a MAVLink packet
    gcs().send_text(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
                                     (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
                                     (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
                                     (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
                                     (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
                                     (double)expected_travel_distance);
    gcs().send_text(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}

/*
  send PID tuning message
 */
void Rover::send_pid_tuning(mavlink_channel_t chan)
{
    const Vector3f &gyro = ahrs.get_gyro();
    if (g.gcs_pid_mask & 1) {
        const DataFlash_Class::PID_Info &pid_info = steerController.get_pid_info();
        mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
                                    pid_info.desired,
                                    degrees(gyro.z),
                                    pid_info.FF,
                                    pid_info.P,
                                    pid_info.I,
                                    pid_info.D);
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
            return;
        }
    }
}

TEST_P (RandomWalkerTest, BuildLinearSystem)
{
  RandomWalker rw (g.graph,
                   boost::get (boost::edge_weight, g.graph),
                   boost::get (boost::vertex_color, g.graph));

  rw.computeVertexDegrees ();
  rw.buildLinearSystem ();

  ASSERT_EQ (g.rows, rw.L.rows ());
  ASSERT_EQ (g.rows, rw.L.cols ());
  ASSERT_EQ (g.rows, rw.B.rows ());
  ASSERT_EQ (g.cols, rw.B.cols ());

  std::vector<Weight> degrees (g.rows, 0.0);
  std::vector<Weight> L_sums (g.rows, 0.0);
  std::vector<Weight> B_sums (g.rows, 0.0);
  for (int k = 0; k < rw.L.outerSize (); ++k)
  {
    for (SparseMatrix::InnerIterator it (rw.L, k); it; ++it)
    {
      EXPECT_GE (it.row (), it.col ()); // the matrix should be lower triangular
      if (it.row () == it.col ())
      {
        degrees[it.row ()] = it.value ();
      }
      else
      {
        L_sums[it.row ()] -= it.value ();
        L_sums[it.col ()] -= it.value ();
      }
    }
  }
  for (int k = 0; k < rw.B.outerSize (); ++k)
  {
    for (SparseMatrix::InnerIterator it (rw.B, k); it; ++it)
    {
      B_sums[it.row ()] += it.value ();
    }
  }
  for (size_t i = 0; i < g.rows; ++i)
  {
    float sum = L_sums[i] + B_sums[i];
    EXPECT_FLOAT_EQ (degrees[i], sum);
  }
}

/*
 Calculate the angles needed to raise the leg from standing position to a certain vertical height (mInches)
 
 */
void  glStaticMovesRobot::lift_leg_from_standing      ( float mInches, bool mLeftLeg )
{
    if (mInches<0)  return ;
        
    float upper_lower_length = m_left_leg.m_upper_leg.m_joint_length + m_left_leg.m_lower_leg.m_joint_length;
    float upper_distance     = mInches * (m_left_leg.m_upper_leg.m_joint_length / upper_lower_length);
    float upper_vertical     = m_left_leg.m_upper_leg.m_joint_length - upper_distance;
    
    // Now find the angle needed to raise upper leg from m_upper_leg.m_joint_length by upper_distance;
    float hypotenus = m_left_leg.m_upper_leg.m_joint_length;
    
    // upper_vertical = hypotenus * cos(mAngle);
    float hip_angle = degrees( acos(upper_vertical / hypotenus) );
    if (mLeftLeg)
        lift_left_leg(hip_angle);
    else
        lift_right_leg(hip_angle);
}

/*
   update location from position
*/
void Aircraft::update_position(void)
{
    float bearing = degrees(atan2f(position.y, position.x));
    float distance = sqrtf(sq(position.x) + sq(position.y));

    location = home;
    location_update(location, bearing, distance);

    location.alt  = home.alt - position.z*100.0f;

    // we only advance time if it hasn't been advanced already by the
    // backend
    if (last_time_us == time_now_us) {
        time_now_us += frame_time_us;
    }
    last_time_us = time_now_us;
    sync_frame_time();
}

void create_projected_internal_degrees(const std::set< edge_t > edge_list, const spa_network_t & network, std::vector<std::vector<unsigned int> > & projected_internal_degrees) {
  projected_internal_degrees.clear();
  projected_internal_degrees.resize(network.size_in_clusters());
  for (id_t cluster = 0 ; cluster < network.size_in_clusters(); ++cluster) {
    LOGGER_WRITE(Logger::DEBUG,VAR_PRINT(cluster))
    // Fetch subgraph info
    std::multiset<edge_t> duplicate_edges = network.get_subgraph(cluster).get_global_links();
    std::set<id_t> nodes = network.get_subgraph(cluster).get_global_ids();
    std::map<id_t, id_t> global_to_local;
    

    // Keep a single copy of each internal edge 
    LOGGER_WRITE(Logger::DEBUG,"Keep a single copy of each internal edge.")
    std::set<edge_t> unique_edges;
    for (auto it = duplicate_edges.begin(); it!=duplicate_edges.end(); ++it) 
      unique_edges.insert(*it);
    duplicate_edges.clear();

    // Project the network onto the subgraph (and build global_to_local map)
    LOGGER_WRITE(Logger::DEBUG,"Project the network onto the subgraph")
    id_t id_counter = 0;
    for (auto it1 = nodes.begin(); it1 != nodes.end();++it1) {
      global_to_local[*it1] = id_counter;
      for (auto it2 = std::next(it1); it2 != nodes.end();++it2) {
        edge_t edge = *it1 < *it2 ? std::make_pair(*it1,*it2) : std::make_pair(*it2,*it1);
        if (edge_list.find(edge)!=edge_list.end()) {
          unique_edges.insert(edge);
        }
      }
      ++id_counter;
    }

    // Get degrees
    LOGGER_WRITE(Logger::DEBUG,"Get degrees")
    std::vector< unsigned int > degrees(nodes.size(),0);
    for (auto it = unique_edges.begin();it!=unique_edges.end();++it) {
      ++degrees[global_to_local.at(it->first)];
      ++degrees[global_to_local.at(it->second)];
    }

    projected_internal_degrees[cluster] = degrees;
  }
  return;
}

void AP_Landing_Deepstall::build_approach_path(void)
{
    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);

    Vector3f wind = landing.ahrs.wind_estimate();
    // TODO: Support a user defined approach heading
    target_heading_deg = (degrees(atan2f(-wind.y, -wind.x)));

    memcpy(&extended_approach, &landing_point, sizeof(Location));
    memcpy(&arc_exit, &landing_point, sizeof(Location));

    //extend the approach point to 1km away so that there is always a navigational target
    location_update(extended_approach, target_heading_deg, 1000.0);

    float expected_travel_distance = predict_travel_distance(wind, landing_point.alt * 0.01f, false);
    float approach_extension_m = expected_travel_distance + approach_extension;
    // an approach extensions must be at least half the loiter radius, or the aircraft has a
    // decent chance to be misaligned on final approach
    approach_extension_m = MAX(approach_extension_m, loiter_radius * 0.5f);

    location_update(arc_exit, target_heading_deg + 180, approach_extension_m);
    memcpy(&arc, &arc_exit, sizeof(Location));
    memcpy(&arc_entry, &arc_exit, sizeof(Location));

    // TODO: Support loitering on either side of the approach path
    location_update(arc, target_heading_deg + 90.0, loiter_radius);
    location_update(arc_entry, target_heading_deg + 90.0, loiter_radius * 2);

#ifdef DEBUG_PRINTS
    // TODO: Send this information via a MAVLink packet
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
                                     (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
                                     (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
                                     (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
                                     (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
                                     (double)expected_travel_distance);
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}

/*
  calculate a new aerodynamic_load_factor and limit nav_roll_cd to
  ensure that the load factor does not take us below the sustainable
  airspeed
 */
void Plane::update_load_factor(void)
{
    float demanded_roll = fabsf(nav_roll_cd*0.01f);
    if (demanded_roll > 85) {
        // limit to 85 degrees to prevent numerical errors
        demanded_roll = 85;
    }
    aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));

    if (!aparm.stall_prevention) {
        // stall prevention is disabled
        return;
    }
    if (fly_inverted()) {
        // no roll limits when inverted
        return;
    }
    if (quadplane.tailsitter_active()) {
        // no limits while hovering
        return;
    }
       

    float max_load_factor = smoothed_airspeed / aparm.airspeed_min;
    if (max_load_factor <= 1) {
        // our airspeed is below the minimum airspeed. Limit roll to
        // 25 degrees
        nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
        roll_limit_cd = constrain_int32(roll_limit_cd, -2500, 2500);
    } else if (max_load_factor < aerodynamic_load_factor) {
        // the demanded nav_roll would take us past the aerodymamic
        // load limit. Limit our roll to a bank angle that will keep
        // the load within what the airframe can handle. We always
        // allow at least 25 degrees of roll however, to ensure the
        // aircraft can be maneuvered with a bad airspeed estimate. At
        // 25 degrees the load factor is 1.1 (10%)
        int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;
        if (roll_limit < 2500) {
            roll_limit = 2500;
        }
        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
        roll_limit_cd = constrain_int32(roll_limit_cd, -roll_limit, roll_limit);
    }    
}

void GCS_MAVLINK_Rover::send_nav_controller_output() const
{
    if (!rover.control_mode->is_autopilot_mode()) {
        return;
    }

    const Mode *control_mode = rover.control_mode;

    mavlink_msg_nav_controller_output_send(
        chan,
        0,  // roll
        degrees(rover.g2.attitude_control.get_desired_pitch()),
        control_mode->nav_bearing(),
        control_mode->wp_bearing(),
        MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
        0,
        control_mode->speed_error(),
        control_mode->crosstrack_error());
}

float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) 
{
  // returns course in degrees (North=0, West=270) from position 1 to position 2,
  // both specified as signed decimal-degrees latitude and longitude.
  // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
  // Courtesy of Maarten Lamers
  float dlon = radians(long2-long1);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  float a1 = sin(dlon) * cos(lat2);
  float a2 = sin(lat1) * cos(lat2) * cos(dlon);
  a2 = cos(lat1) * sin(lat2) - a2;
  a2 = atan2(a1, a2);
  if (a2 < 0.0)
  {
    a2 += TWO_PI;
  }
  return degrees(a2);
}

// correct a bearing in centi-degrees for wind
void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd)
{
    if (!use_compass() || !_flags.wind_estimation) {
        // we are not using the compass - no wind correction,
        // as GPS gives course over ground already
        return;
    }

    // if we are using a compass for navigation, then adjust the
    // heading to account for wind
    Vector3f wind = wind_estimate();
    Vector2f wind2d = Vector2f(wind.x, wind.y);
    float speed;
    if (airspeed_estimate(&speed)) {
        Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
        Vector2f nav_adjusted = nav_vector - wind2d;
        nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
    }
}

// runs a seasonally forced gillespie simulation on a exponential network
int main(int argc,char *argv[]) {
    if(argc != 9) {
            printf("Wrong Number of Arguments\n");
            exit(0);
    }

    double alpha1, alpha2, beta1_max, beta2_max, gamma1, gamma2, phi1, phi2, eta1, eta2;
    alpha1 = alpha2 = atof( argv[1] );
    beta1_max = atof( argv[2] );  
    beta2_max = atof( argv[3] );
    eta1 = eta2 = 1.0/2.62;
    gamma1 = gamma2 = 1.0/3.38;
    phi1 = phi2 = atof( argv[4]);
    int intro_time, start_ind, shift;
    intro_time = atoi( argv[5] );
    start_ind = atoi( argv[6] );
    shift = atoi( argv[7] );
    string network_type = argv[8];
    
    int num_reps = 4000;

    Network net = Network("gillespie toy", Network::Undirected);
    net.populate(10000);
    for(int i =1; i <= num_reps; i++){
        net.clear_edges();
        if(network_type == "exp"){
            net.rand_connect_exponential(0.06453487);
        } else if(network_type == "unif"){
            vector<int> degrees(10000, 16);
            net.rand_connect_explicit(degrees);
        } else{
            cerr << "Unrecognized network type" << endl;
        }
        // cout << net.mean_deg() << endl;
        Gillespie_SEIR_TwoStrain_Network sim(&net, alpha1, alpha2, eta1, eta2, gamma1, gamma2, beta1_max, beta2_max, phi1, phi2, intro_time, start_ind, shift);
        cout << "Simulation number: " << i << endl;
        sim.reset();
        sim.rand_infect(5, 1);
        sim.run_simulation(10000.0);
    }
    return 0;
}

/*
*  Called to determine if Manual CR Pitch has hit range limits
*/
void Tracker::check_manual_pitch_limits(int16_t pitch_pwm)
{
    //Check measured pitch angle of ahrs
    float ahrs_pitch = degrees(ahrs.pitch);
    //float pitch_range = g.pitch_range/2;	

    if (ahrs_pitch >= g.pitch_max) { //MAX LIMIT
        //Check if servo is reversed and limit servo in one direction only
        if (!RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm < RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Normal Servo Direction
            pitch_lock = false;
        } else if (RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm > RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Reversed Servo Direction
            pitch_lock = false;
            } else {
            pitch_lock_angle = g.pitch_max;
            pitch_lock = true; //Stop if not in a direction to clear violation
        }
        return;
    }

    if (ahrs_pitch <= g.pitch_min) { //MIN LIMIT
        //Check if servo is reversed and limit servo in one direction only
        if (!RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm > RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Normal Servo Direction
            pitch_lock = false;
        } else if (RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm < RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Reversed Servo Direction
            pitch_lock = false;
        } else {
            pitch_lock_angle = g.pitch_min;
            pitch_lock = true; //Stop if not in a direction to clear violation
        }
        return;
    }

    if (pitch_pwm > (RC_Channels::rc_channel(CH_PITCH)->get_radio_trim() - 10) && pitch_pwm < (RC_Channels::rc_channel(CH_PITCH)->get_radio_trim() + 10)) {
        if (!pitch_lock) {
        pitch_lock_angle = ahrs_pitch;
        pitch_lock = true;
        }
        return;
    } 

    pitch_lock = false; //Not in pitch violation
}

int main(int argc,char *argv[]) {
    if(argc != 10) {
            printf("Wrong Number of Arguments\n");
            exit(0);
    }

    //double alpha1 = argv[1]-0.0;
    double beta_vax, beta_dis, gamma_vax, gamma_dis;
    int init_inf, init_vax;

    beta_vax = atof( argv[1] ); // vaccine transmissibility
    beta_dis = atof( argv[2] ); // disease transmissibility
    gamma_vax = atof( argv[3] ); // vaccine recovery rate
    gamma_dis = atof( argv[4] ); // disease recovery rate
    init_vax = atoi( argv[5] ); // number initially vaccinated
    init_inf = atoi( argv[6] ); // number initially infected

    string network_type = argv[7] ;
    string vax_type = argv[8];
    int num_reps = atoi( argv[9]);

    Network net = Network("gillespie toy", Network::Undirected);
    net.populate(10000);
    for(int i =1; i <= num_reps; i++){
        net.clear_edges();
        if(network_type == "er"){
            net.erdos_renyi(10);
        } else if(network_type == "exp"){
            net.rand_connect_exponential(0.1);
        } else if(network_type == "unif"){
            vector<int> degrees(10000, 10);
            net.rand_connect_explicit(degrees);
        } else{
            cerr << "Unrecognized network type" << endl;
        }
        Gillespie_trans_vaccine sim(&net, gamma_vax, gamma_dis, beta_vax, beta_dis, vax_type);
        cout << "Simulation number: " << i << endl;
        sim.reset();
        sim.run_simulation(10000.0, init_vax, init_inf);
    }
    return 0;
}

int EEregionCheck (int ics, int ips,
                   int radStart, int radEnd,
                   int phiStart, int phiEnd)  
{
  int x = ics-50;
  int y = ips-50;
  double radius2 = x*x + y*y ;
  if (radius2 < 10*10) return 1;  //center of the donut
  if (radius2 > 50*50) return 1;  //outer part of the donut
  if (radius2 < radStart * radStart) return 2 ;
  if (radius2 >= radEnd * radEnd) return 2 ;
  double phi = atan2 (static_cast<double> (y),static_cast<double> (x));
  phi = degrees (phi);
  if (phi < 0) phi += 360; 
  if (phiStart < phiEnd 
     && phi > phiStart && phi < phiEnd ) return 0; 
  if (phiStart > phiEnd 
      && (phi > phiStart|| phi < phiEnd )) return 0; 
   return 3;
}

void CubeMovingState::Execute(float dt)
{
    BSpline* spline = cubeModel->GetSpline();
    if (spline != nullptr)
    {
        cubeModel->SetPosition(spline->GetPosition() + spline->GetNextPoint());
        if (cubeModel->LooksForward())
        {
            glm::vec3 velocity = glm::vec3(spline->GetVelocityUnitVector());
            glm::vec3 forwardVector = cubeModel->mForward;
            glm::vec3 rotationAxis = glm::cross(forwardVector, velocity);
            float rotationAngleInDegrees = degrees(glm::acos(glm::dot(forwardVector, velocity) / (length(forwardVector) * length(velocity))));
            
            cubeModel->SetRotation(rotationAxis, rotationAngleInDegrees);
        }
        spline->Update(dt);
    }

    duration -= dt;
}

 int order(const RCP<OrderingSolution<lno_t, gno_t> > &solution)
 {
   int ierr= 0;
 
   HELLO;
 
   lno_t *perm;
   perm = (lno_t *) (solution->getPermutation());
   if (perm==0){
     // Throw exception
     std::cerr << "perm is NULL" << std::endl;
     ierr = -1;
   }
 
   // Get local graph.
   const size_t nVtx = model->getLocalNumVertices();
   ArrayView<const gno_t> edgeIds;
   ArrayView<const lno_t> offsets;
   ArrayView<StridedData<lno_t, scalar_t> > wgts;
   model->getEdgeList(edgeIds, offsets, wgts);
 
   // Store degrees together with index so we can sort.
   Teuchos::Array<std::pair<lno_t, size_t> >  degrees(nVtx);
   for (lno_t i=0; i<(lno_t)nVtx; i++){
     degrees[i].first = i;
     degrees[i].second  = offsets[i+1] - offsets[i];
   }
 
   // Sort degrees.
   SortPairs<lno_t,size_t> zort;
   bool inc = true; // TODO: Add user parameter
   zort.sort(degrees, inc);
 
   // Copy permuted indices to perm.
   for (lno_t i=0; i<(lno_t)nVtx; i++){
     perm[i] = degrees[i].first;
   }
 
   solution->setHavePerm(true);
   return ierr;
 }