#include "cpp_bmad_classes.h"
#include "erl_doocs_bmad_interface.h"
#include <iostream>

using namespace std;

extern "C" void c_side_() {

  int i_lat;
  bool err_flag;
  double e_tot;

  bmad_create_init_global_(10, 10);

  i_lat = 0;
  while (true) {

    CPP_ele ele, bend, ele2;

    i_lat++;
    if (i_lat > 3) i_lat = 1;

    bmad_remove_lat_(i_lat);
    bmad_create_init_lat_(i_lat, 3, 0.0, 15.0, 1.0e7, err_flag);

    ele.key = Bmad::LCAVITY;
    ele.name = "C1";
    ele.value[Bmad::L] = 0.4;
    ele.value[Bmad::RF_FREQUENCY] = 1.3e9;
    ele.s = 4;
    ele.field_master = true;

    bmad_create_element_(i_lat, ele, false, "erl_doocs_test/ele_field.bmad", err_flag);

    ele.name = "CAV";
    ele.key = Bmad::LCAVITY;
    ele.s = 4.4;

    bmad_create_element_(i_lat, ele, false, "", err_flag);

    bend.name = "BND";
    bend.key = Bmad::SBEND;
    bend.value[Bmad::G] = 0.01;
    bend.value[Bmad::L] = 1.2;
    bend.s = 9.7;

    bmad_create_bend_(i_lat, bend, err_flag);

    bmad_create_finalize_(i_lat, err_flag);

    CPP_twiss a_twiss, b_twiss;
    double orbit[6];
    bmad_return_twiss_and_track_(i_lat, 2, 3.7, a_twiss, b_twiss, orbit, e_tot, err_flag);
    bmad_return_twiss_and_track_(i_lat, 2, 3.8, a_twiss, b_twiss, orbit, e_tot, err_flag);

//    cout << "Beta_a: " << a_twiss.beta << endl;

    int ix_ele1 = bmad_element_index_(i_lat, "BND");
    bmad_modify_element_attribute_(i_lat, ix_ele1, Bmad::RHO, 0.1);
    bmad_lattice_bookkeeper_(i_lat, err_flag);

    double vec0[6], mat6_vec[36];
    bmad_calc_transfer_matrix_(i_lat, 0.0, 15.0, vec0, mat6_vec, err_flag);

    //    cout << "q1 index: " << bmad_element_index_(i_lat, "q1") << endl;
    bmad_set_twiss_at_beginning_(i_lat, 3.8, a_twiss, b_twiss);

    bmad_write_lattice_to_file_(i_lat, "abc", 3);
    bmad_return_element_(i_lat, 2, ele, err_flag);

    // bunch test

    CPP_bunch bunch, bunch_end;
    bunch.particle.resize(10);
    bunch.particle[0].vec[4] = -1e-3;
    bunch.particle[1].vec[4] = 2e-3;
    bunch.particle[2].vec[4] = 1e-3;
    bunch.particle[3].vec[4] = -2e-3;
    for (unsigned int i = 0; i < bunch.particle.size(); i++) {
      bmad_init_coord_(1, 0, bunch.particle[i]);
    }
    bmad_track1_bunch_(1, 1, bunch, bunch_end, err_flag);
    cout << bunch_end.particle[1].vec[2];

  }

}