/* siman/gsl_siman.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Mark Galassi
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_SIMAN_H__
#define __GSL_SIMAN_H__
#include <stdlib.h>
#include <gsl/gsl_rng.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/* types for the function pointers passed to gsl_siman_solve */

typedef double (*gsl_siman_Efunc_t) (void *xp);
typedef void (*gsl_siman_step_t) (const gsl_rng *r, void *xp, double step_size);
typedef double (*gsl_siman_metric_t) (void *xp, void *yp);
typedef void (*gsl_siman_print_t) (void *xp);
typedef void (*gsl_siman_copy_t) (void *source, void *dest);
typedef void * (*gsl_siman_copy_construct_t) (void *xp);
typedef void (*gsl_siman_destroy_t) (void *xp);

/* this structure contains all the information needed to structure the
   search, beyond the energy function, the step function and the
   initial guess. */

typedef struct {
  int n_tries;          /* how many points to try for each step */
  int iters_fixed_T;    /* how many iterations at each temperature? */
  double step_size;     /* max step size in the random walk */
  /* the following parameters are for the Boltzmann distribution */
  double k, t_initial, mu_t, t_min;
} gsl_siman_params_t;

/* prototype for the workhorse function */

void gsl_siman_solve(const gsl_rng * r,
                     void *x0_p, gsl_siman_Efunc_t Ef,
                     gsl_siman_step_t take_step,
                     gsl_siman_metric_t distance,
                     gsl_siman_print_t print_position,
                     gsl_siman_copy_t copyfunc,
                     gsl_siman_copy_construct_t copy_constructor,
                     gsl_siman_destroy_t destructor,
                     size_t element_size,
                     gsl_siman_params_t params);

void
gsl_siman_solve_many (const gsl_rng * r, void *x0_p, gsl_siman_Efunc_t Ef,
                      gsl_siman_step_t take_step,
                      gsl_siman_metric_t distance,
                      gsl_siman_print_t print_position,
                      size_t element_size,
                      gsl_siman_params_t params);

__END_DECLS

#endif /* __GSL_SIMAN_H__ */


/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.1  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

#include <string.h>
#include <math.h>
#include <unistd.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <cblas.h>
#include <execinfo.h>

//<<PAD>>//
//#include <mpi.h>
//#include "UTIL/FIFO/pad_list.h"
#include "discrete_event_generator.h"
#include "threadpool.h"
#include <pthread.h>
#include "oaisim_functions.h"
//<<PAD>>//

#include "SIMULATION/RF/defs.h"
#include "PHY/types.h"
#include "PHY/defs.h"
#include "PHY/vars.h"
#include "MAC_INTERFACE/vars.h"

//#ifdef OPENAIR2
#include "LAYER2/MAC/defs.h"
#include "LAYER2/MAC/vars.h"
#include "RRC/LITE/vars.h"
#include "PHY_INTERFACE/vars.h"
//#endif

#include "ARCH/CBMIMO1/DEVICE_DRIVER/vars.h"

#ifdef IFFT_FPGA
//#include "PHY/LTE_REFSIG/mod_table.h"
#endif //IFFT_FPGA

#include "SCHED/defs.h"
#include "SCHED/vars.h"

#include "oaisim.h"
#include "oaisim_config.h"
#include "UTIL/OCG/OCG_extern.h"
#include "cor_SF_sim.h"
#include "UTIL/OMG/omg_constants.h"

//#include "UTIL/LOG/vcd_signal_dumper.h"

#define RF

//#define DEBUG_SIM

#define MCS_COUNT 24//added for PHY abstraction
#define N_TRIALS 1

/*
  DCI0_5MHz_TDD0_t          UL_alloc_pdu;
  DCI1A_5MHz_TDD_1_6_t      CCCH_alloc_pdu;
  DCI2_5MHz_2A_L10PRB_TDD_t DLSCH_alloc_pdu1;
  DCI2_5MHz_2A_M10PRB_TDD_t DLSCH_alloc_pdu2;
 */

#define UL_RB_ALLOC computeRIV(lte_frame_parms->N_RB_UL,0,24)
#define CCCH_RB_ALLOC computeRIV(lte_frame_parms->N_RB_UL,0,3)
#define RA_RB_ALLOC computeRIV(lte_frame_parms->N_RB_UL,0,3)
#define DLSCH_RB_ALLOC 0x1fff

#define DECOR_DIST 100
#define SF_VAR 10

//constant for OAISIM soft realtime calibration
#define SF_DEVIATION_OFFSET_NS 100000 //= 0.1ms : should be as a number of UE
#define SLEEP_STEP_US       100 //  = 0.01ms could be adaptive, should be as a number of UE
#define K 2                  // averaging coefficient
#define TARGET_SF_TIME_NS 1000000       // 1ms = 1000000 ns

//#ifdef OPENAIR2
//uint16_t NODE_ID[1];
//uint8_t NB_INST = 2;
//#endif //OPENAIR2
extern int otg_times;
extern int for_times;
extern int if_times;
int for_main_times = 0;

frame_t frame=0;
char stats_buffer[16384];
channel_desc_t *eNB2UE[NUMBER_OF_eNB_MAX][NUMBER_OF_UE_MAX];
channel_desc_t *UE2eNB[NUMBER_OF_UE_MAX][NUMBER_OF_eNB_MAX];
Signal_buffers *signal_buffers_g;
//Added for PHY abstraction
node_desc_t *enb_data[NUMBER_OF_eNB_MAX];
node_desc_t *ue_data[NUMBER_OF_UE_MAX];
//double sinr_bler_map[MCS_COUNT][2][16];
//double sinr_bler_map_up[MCS_COUNT][2][16];
//extern double SINRpost_eff[301];
extern int mcsPost;
extern int  nrbPost;
extern int frbPost;
extern void kpi_gen();

extern uint16_t Nid_cell;
extern uint8_t target_dl_mcs;
extern uint8_t rate_adaptation_flag;
extern double snr_dB, sinr_dB;
extern uint8_t set_seed;
extern uint8_t cooperation_flag;          // for cooperative communication
extern uint8_t abstraction_flag, ethernet_flag;
extern uint8_t ue_connection_test;
extern int map1,map2;
extern double **ShaF;
// pointers signal buffers (s = transmit, r,r0 = receive)
extern double **s_re, **s_im, **r_re, **r_im, **r_re0, **r_im0;
extern Node_list ue_node_list;
extern Node_list enb_node_list;
extern int pdcp_period, omg_period;
extern LTE_DL_FRAME_PARMS *frame_parms;
// time calibration for soft realtime mode
extern struct timespec time_spec;
extern unsigned long time_last, time_now;
extern int td, td_avg, sleep_time_us;

int eMBMS_active = 0;

threadpool_t * pool;

#ifdef OPENAIR2
extern int pfd[2];
#endif

// this should reflect the channel models in openair1/SIMULATION/TOOLS/defs.h
mapping small_scale_names[] = {
  {"custom", custom},
  {"SCM_A", SCM_A},
  {"SCM_B", SCM_B},
  {"SCM_C", SCM_C},
  {"SCM_D", SCM_D},
  {"EPA", EPA},
  {"EVA", EVA},
  {"ETU", ETU},
  {"Rayleigh8", Rayleigh8},
  {"Rayleigh1", Rayleigh1},
  {"Rayleigh1_800", Rayleigh1_800},
  {"Rayleigh1_corr", Rayleigh1_corr},
  {"Rayleigh1_anticorr", Rayleigh1_anticorr},
  {"Rice8", Rice8},
  {"Rice1", Rice1},
  {"Rice1_corr", Rice1_corr},
  {"Rice1_anticorr", Rice1_anticorr},
  {"AWGN", AWGN},
  {NULL, -1}
};

//static void *sigh(void *arg);
void terminate(void);

void
help (void)
{
  printf
  ("Usage: oaisim -h -a -F -C tdd_config -V -R N_RB_DL -e -x transmission_mode -m target_dl_mcs -r(ate_adaptation) -n n_frames -s snr_dB -k ricean_factor -t max_delay -f forgetting factor -A channel_model -z cooperation_flag -u nb_local_ue -U UE mobility -b nb_local_enb -B eNB_mobility -M ethernet_flag -p nb_master -g multicast_group -l log_level -c ocg_enable -T traffic model -D multicast network device\n");

  printf ("-h provides this help message!\n");
  printf ("-a Activates PHY abstraction mode\n");
  printf ("-F Activates FDD transmission (TDD is default)\n");
  printf ("-C [0-6] Sets TDD configuration\n");
  printf ("-R [6,15,25,50,75,100] Sets N_RB_DL\n");
  printf ("-e Activates extended prefix mode\n");
  printf ("-m Gives a fixed DL mcs\n");
  printf ("-r Activates rate adaptation (DL for now)\n");
  printf ("-n Set the number of frames for the simulation\n");
  printf ("-s snr_dB set a fixed (average) SNR, this deactivates the openair channel model generator (OCM)\n");
  printf ("-S snir_dB set a fixed (average) SNIR, this deactivates the openair channel model generator (OCM)\n");
  printf ("-k Set the Ricean factor (linear)\n");
  printf ("-t Set the delay spread (microseconds)\n");
  printf ("-f Set the forgetting factor for time-variation\n");
  printf ("-A set the multipath channel simulation,  options are: SCM_A, SCM_B, SCM_C, SCM_D, EPA, EVA, ETU, Rayleigh8, Rayleigh1, Rayleigh1_corr,Rayleigh1_anticorr, Rice8,, Rice1, AWGN \n");
  printf ("-b Set the number of local eNB\n");
  printf ("-u Set the number of local UE\n");
  printf ("-M Set the machine ID for Ethernet-based emulation\n");
  printf ("-p Set the total number of machine in emulation - valid if M is set\n");
  printf ("-g Set multicast group ID (0,1,2,3) - valid if M is set\n");
  printf ("-l Set the global log level (8:trace, 7:debug, 6:info, 4:warn, 3:error) \n");
  printf
  ("-c [1,2,3,4] Activate the config generator (OCG) to process the scenario descriptor, or give the scenario manually: -c template_1.xml \n");
  printf ("-x Set the transmission mode (1,2,5,6 supported for now)\n");
  printf ("-z Set the cooperation flag (0 for no cooperation, 1 for delay diversity and 2 for distributed alamouti\n");
  printf ("-T activate the traffic generator: 0 for NONE, 1 for CBR, 2 for M2M, 3 for FPS Gaming, 4 for mix\n");
  printf ("-B Set the mobility model for eNB, options are: STATIC, RWP, RWALK, \n");
  printf ("-U Set the mobility model for UE, options are: STATIC, RWP, RWALK \n");
  printf ("-E Random number generator seed\n");
  printf ("-P enable protocol analyzer : 0 for wireshark interface, 1: for pcap , 2 : for tshark \n");
  printf ("-I Enable CLI interface (to connect use telnet localhost 1352)\n");
  printf ("-V Enable VCD dump, file = openair_vcd_dump.vcd\n");
  printf ("-G Enable background traffic \n");
  printf ("-O [mme ipv4 address] Enable MME mode\n");
  printf ("-Z Reserved\n");
}

#ifdef OPENAIR2
void omv_end (int pfd, Data_Flow_Unit omv_data);
int omv_write (int pfd,  Node_list enb_node_list, Node_list ue_node_list, Data_Flow_Unit omv_data);
#endif

//<<<< PAD >>>>//
#define PAD 1
//#define PAD_FINE 1
//#define PAD_SYNC 1
#define JOB_REQUEST_TAG 246
#define JOB_REPLY_TAG 369
#define FRAME_END 888
#define NO_JOBS_TAG 404
#define JOB_DIST_DEBUG 33

//Global Variables
int worker_number;
int frame_number = 1;
//<<<< PAD >>>>//

//<<<< DEG >>>>//
extern End_Of_Sim_Event end_event; //Could later be a list of condition_events
extern Event_List event_list;
//<<<< DEG >>>>//

extern Packet_OTG_List *otg_pdcp_buffer;

void run(int argc, char *argv[]);

#ifdef PAD
void pad_init()
{

  int UE_id, i;

  pool = threadpool_create(PAD);

  if (pool == NULL) {
    printf("ERROR threadpool allocation\n");
    return;
  }

  signal_buffers_g = malloc(NB_UE_INST * sizeof(Signal_buffers));

  if (abstraction_flag == 0) {
    for (UE_id = 0; UE_id < NB_UE_INST; UE_id++) {
      signal_buffers_g[UE_id].s_re = malloc(2*sizeof(double*));
      signal_buffers_g[UE_id].s_im = malloc(2*sizeof(double*));
      signal_buffers_g[UE_id].r_re = malloc(2*sizeof(double*));
      signal_buffers_g[UE_id].r_im = malloc(2*sizeof(double*));
      signal_buffers_g[UE_id].r_re0 = malloc(2*sizeof(double*));
      signal_buffers_g[UE_id].r_im0 = malloc(2*sizeof(double*));

      for (i=0; i<2; i++) {

        signal_buffers_g[UE_id].s_re[i] = malloc(FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        bzero(signal_buffers_g[UE_id].s_re[i],FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        signal_buffers_g[UE_id].s_im[i] = malloc(FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        bzero(signal_buffers_g[UE_id].s_im[i],FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        signal_buffers_g[UE_id].r_re[i] = malloc(FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        bzero(signal_buffers_g[UE_id].r_re[i],FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        signal_buffers_g[UE_id].r_im[i] = malloc(FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        bzero(signal_buffers_g[UE_id].r_im[i],FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        signal_buffers_g[UE_id].r_re0[i] = malloc(FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        bzero(signal_buffers_g[UE_id].r_re0[i],FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        signal_buffers_g[UE_id].r_im0[i] = malloc(FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
        bzero(signal_buffers_g[UE_id].r_im0[i],FRAME_LENGTH_COMPLEX_SAMPLES*sizeof(double));
      }
    }
  }
}

void pad_finalize()
{

  int ret, i;
  module_id_t UE_id;

  ret = threadpool_destroy(pool);

  if (ret)
    printf("ERROR threadpool destroy = %d\n", ret);

  if (abstraction_flag == 0) {

    for (UE_id = 0; UE_id < NB_UE_INST; UE_id++) {
      for (i = 0; i < 2; i++) {
        free(signal_buffers_g[UE_id].s_re[i]);
        free(signal_buffers_g[UE_id].s_im[i]);
        free(signal_buffers_g[UE_id].r_re[i]);
        free(signal_buffers_g[UE_id].r_im[i]);
      }

      free(signal_buffers_g[UE_id].s_re);
      free(signal_buffers_g[UE_id].s_im);
      free(signal_buffers_g[UE_id].r_re);
      free(signal_buffers_g[UE_id].r_im);
    }

    //free node by node here same pattern as below
  }

  free(signal_buffers_g);
}

void pad_inject_job(int eNB_flag, int nid, int frame, int next_slot, int last_slot, enum Job_type type, int ctime)
{

  int ret;
  Job_elt *job_elt;

  job_elt = malloc(sizeof(Job_elt));
  job_elt->next = NULL;
  (job_elt->job).eNB_flag = eNB_flag;
  (job_elt->job).nid = nid;
  (job_elt->job).frame = frame;
  (job_elt->job).next_slot = next_slot;
  (job_elt->job).last_slot = last_slot;
  (job_elt->job).type = type;
  (job_elt->job).ctime = ctime;

  ret = threadpool_add(pool, job_elt);

  if (ret) {
    printf("ERROR threadpool_add %d\n", ret);
    return;
  }
}

void pad_synchronize()
{
  pthread_mutex_lock(&(pool->sync_lock));

  while(pool->active > 0) {
    pthread_cond_wait(&(pool->sync_notify), &(pool->sync_lock));
  }

  pthread_mutex_unlock(&(pool->sync_lock));
}

#endif
//<<PAD(DEG_MAIN)>>//
int main (int argc, char *argv[])
{

  //Mobility *mobility_frame_10;
  //Application_Config *application_frame_20;

  //Here make modifications on the mobility and traffic new models
  //mob_frame_10 -> ...
  //application_frame_30 -> ...

  //schedule(ET_OMG, 10, NULL, mobility_frame_10);
  //schedule(ET_OTG, 15, NULL, application_frame_20);

  //event_list_display(&event_list);

  schedule_end_of_simulation(FRAME, 100);

  run(argc, argv);

  return 0;
}
//<<PAD>>//

//<<PAD(RUN)>>//
void run(int argc, char *argv[])
{

  int32_t i;
  module_id_t UE_id, eNB_id;
  Job_elt *job_elt;
  int ret;

  clock_t t;

  Event_elt *user_defined_event;
  Event event;

  // Framing variables
  int32_t slot, last_slot, next_slot;

  FILE *SINRpost;
  char SINRpost_fname[512];
  sprintf(SINRpost_fname,"postprocSINR.m");
  SINRpost = fopen(SINRpost_fname,"w");
  // variables/flags which are set by user on command-line
  double snr_direction,snr_step=1.0;//,sinr_direction;

  lte_subframe_t direction;
  char fname[64],vname[64];

#ifdef XFORMS
  // current status is that every UE has a DL scope for a SINGLE eNB (eNB_id=0)
  // at eNB 0, an UL scope for every UE
  FD_lte_phy_scope_ue  *form_ue[NUMBER_OF_UE_MAX];
  FD_lte_phy_scope_enb *form_enb[NUMBER_OF_UE_MAX];
  char title[255];
#endif

#ifdef PROC
  int node_id;
  int port,Process_Flag=0,wgt,Channel_Flag=0,temp;
#endif

  // uint8_t awgn_flag = 0;

#ifdef PRINT_STATS
  int len;
  FILE *UE_stats[NUMBER_OF_UE_MAX], *UE_stats_th[NUMBER_OF_UE_MAX], *eNB_stats, *eNB_avg_thr, *eNB_l2_stats;
  char UE_stats_filename[255];
  char UE_stats_th_filename[255];
  char eNB_stats_th_filename[255];
#endif

#ifdef SMBV
  uint8_t config_smbv = 0;
  char smbv_ip[16];
  strcpy(smbv_ip,DEFAULT_SMBV_IP);
#endif

#ifdef OPENAIR2
  Data_Flow_Unit omv_data;
#endif

  //time_t t0,t1;
  //clock_t start, stop;

  //double **s_re2[MAX_eNB+MAX_UE], **s_im2[MAX_eNB+MAX_UE], **r_re2[MAX_eNB+MAX_UE], **r_im2[MAX_eNB+MAX_UE], **r_re02, **r_im02;
  //double **r_re0_d[MAX_UE][MAX_eNB], **r_im0_d[MAX_UE][MAX_eNB], **r_re0_u[MAX_eNB][MAX_UE],**r_im0_u[MAX_eNB][MAX_UE];
  //default parameters

  //{
  /* INITIALIZATIONS */

  target_dl_mcs = 0;
  rate_adaptation_flag = 0;
  oai_emulation.info.n_frames = 0xffff;//1024;          //10;
  oai_emulation.info.n_frames_flag = 0;//fixme
  snr_dB = 30;
  cooperation_flag = 0;         // default value 0 for no cooperation, 1 for Delay diversity, 2 for Distributed Alamouti

  //Default values if not changed by the user in get_simulation_options();
  pdcp_period = 1;
  omg_period = 10;

  mRAL_init_default_values(); //Default values
  eRAL_init_default_values(); //Default values

  init_oai_emulation(); //Default values

  get_simulation_options(argc, argv); //Command-line options

  oaisim_config(); // config OMG and OCG, OPT, OTG, OLG

  //To fix eventual conflict on the value of n_frames
  if (oai_emulation.info.n_frames_flag) {
    schedule_end_of_simulation(FRAME, oai_emulation.info.n_frames);
  }

  VCD_SIGNAL_DUMPER_INIT(); // Initialize VCD LOG module

#ifdef OPENAIR2
  init_omv();
#endif

  check_and_adjust_params(); //Before this call, NB_UE_INST and NB_eNB_INST are not set correctly

  init_otg_pdcp_buffer();

#ifdef PRINT_STATS

  for (UE_id=0; UE_id<NB_UE_INST; UE_id++) {
    sprintf(UE_stats_filename,"UE_stats%d.txt",UE_id);
    UE_stats[UE_id] = fopen (UE_stats_filename, "w");
  }

  eNB_stats = fopen ("eNB_stats.txt", "w");
  printf ("UE_stats=%p, eNB_stats=%p\n", UE_stats, eNB_stats);

  eNB_avg_thr = fopen ("eNB_stats_th.txt", "w");

#endif

  LOG_I(EMU,"total number of UE %d (local %d, remote %d) mobility %s \n", NB_UE_INST,oai_emulation.info.nb_ue_local,oai_emulation.info.nb_ue_remote,
        oai_emulation.topology_config.mobility.eNB_mobility.eNB_mobility_type.selected_option);
  LOG_I(EMU,"Total number of eNB %d (local %d, remote %d) mobility %s \n", NB_eNB_INST,oai_emulation.info.nb_enb_local,oai_emulation.info.nb_enb_remote,
        oai_emulation.topology_config.mobility.UE_mobility.UE_mobility_type.selected_option);
  LOG_I(OCM,"Running with frame_type %d, Nid_cell %d, N_RB_DL %d, EP %d, mode %d, target dl_mcs %d, rate adaptation %d, nframes %d, abstraction %d, channel %s\n",
        oai_emulation.info.frame_type, Nid_cell, oai_emulation.info.N_RB_DL, oai_emulation.info.extended_prefix_flag, oai_emulation.info.transmission_mode,target_dl_mcs,rate_adaptation_flag,
        oai_emulation.info.n_frames,abstraction_flag,oai_emulation.environment_system_config.fading.small_scale.selected_option);

  init_seed(set_seed);

  init_openair1();

  init_openair2();

  init_ocm();

#ifdef XFORMS
  init_xforms();
#endif

  printf ("before L2 init: Nid_cell %d\n", PHY_vars_eNB_g[0]->lte_frame_parms.Nid_cell);
  printf ("before L2 init: frame_type %d,tdd_config %d\n",
          PHY_vars_eNB_g[0]->lte_frame_parms.frame_type,
          PHY_vars_eNB_g[0]->lte_frame_parms.tdd_config);

  init_time();

#ifdef PAD
  pad_init();
#endif

  if (ue_connection_test == 1) {
    snr_direction = -snr_step;
    snr_dB=20;
    sinr_dB=-20;
  }

  frame = 0;
  slot = 0;

  LOG_I(EMU,">>>>>>>>>>>>>>>>>>>>>>>>>>> OAIEMU initialization done <<<<<<<<<<<<<<<<<<<<<<<<<<\n\n");

  printf ("after init: Nid_cell %d\n", PHY_vars_eNB_g[0]->lte_frame_parms.Nid_cell);
  printf ("after init: frame_type %d,tdd_config %d\n",
          PHY_vars_eNB_g[0]->lte_frame_parms.frame_type,
          PHY_vars_eNB_g[0]->lte_frame_parms.tdd_config);

  t = clock();

  while (!end_of_simulation()) {

    last_slot = (slot - 1)%20;

    if (last_slot <0)
      last_slot+=20;

    next_slot = (slot + 1)%20;

    oai_emulation.info.time_ms = frame * 10 + (next_slot>>1);
    oai_emulation.info.frame = frame;

    if (slot == 0) { //Frame's Prologue

      //Run the aperiodic user-defined events
      while ((user_defined_event = event_list_get_head(&event_list)) != NULL) {

        event = user_defined_event->event;

        if (event.frame == frame) {
          switch (event.type) {
          case ET_OMG:
            update_omg_model(event.key, event.value); //implement it with assigning the new values to that of oai_emulation & second thing is to ensure mob model is always read from oai_emulation
            user_defined_event = event_list_remove_head(&event_list);
            break;

          case ET_OTG:
            update_otg_model(event.key, event.value);
            user_defined_event = event_list_remove_head(&event_list);
            break;
          }
        } else {
          break;
        }
      }

      //Comment (handle cooperation flag) deleted here. Look at oaisim.c to see it
      if (ue_connection_test==1) {
        if ((frame%20) == 0) {
          snr_dB += snr_direction;
          sinr_dB -= snr_direction;
        }

        if (snr_dB == -20) {
          snr_direction=snr_step;
        } else if (snr_dB==20) {
          snr_direction=-snr_step;
        }
      }

      update_omg(); // frequency is defined in the omg_global params configurable by the user

      update_omg_ocm();

#ifdef OPENAIR2

      // check if pipe is still open
      if ((oai_emulation.info.omv_enabled == 1) ) {
        omv_write(pfd[1], enb_node_list, ue_node_list, omv_data);
      }

#endif

#ifdef DEBUG_OMG

      if ((((int) oai_emulation.info.time_s) % 100) == 0) {
        for (UE_id = oai_emulation.info.first_ue_local; UE_id < (oai_emulation.info.first_ue_local + oai_emulation.info.nb_ue_local); UE_id++) {
          get_node_position (UE, UE_id);
        }
      }

#endif
      update_ocm();
    }

    direction = subframe_select(frame_parms,next_slot>>1);

    if((next_slot %2) ==0)
      clear_eNB_transport_info(oai_emulation.info.nb_enb_local);

    for (eNB_id=oai_emulation.info.first_enb_local;
         (eNB_id<(oai_emulation.info.first_enb_local+oai_emulation.info.nb_enb_local)) && (oai_emulation.info.cli_start_enb[eNB_id]==1);
         eNB_id++) {
      for_main_times += 1;
      //printf ("debug: Nid_cell %d\n", PHY_vars_eNB_g[eNB_id]->lte_frame_parms.Nid_cell);
      //printf ("debug: frame_type %d,tdd_config %d\n", PHY_vars_eNB_g[eNB_id]->lte_frame_parms.frame_type,PHY_vars_eNB_g[eNB_id]->lte_frame_parms.tdd_config);
      LOG_D(EMU,"PHY procedures eNB %d for frame %d, slot %d (subframe TX %d, RX %d) TDD %d/%d Nid_cell %d\n",
            eNB_id, frame, slot, next_slot >> 1,last_slot>>1,
            PHY_vars_eNB_g[eNB_id]->lte_frame_parms.frame_type,
            PHY_vars_eNB_g[eNB_id]->lte_frame_parms.tdd_config,PHY_vars_eNB_g[eNB_id]->lte_frame_parms.Nid_cell);

      //Appliation
#ifdef PAD_FINE
      pad_inject_job(1, eNB_id, frame, next_slot, last_slot, JT_OTG, oai_emulation.info.time_ms);
#else
      update_otg_eNB(eNB_id, oai_emulation.info.time_ms);
#endif

      //Access layer
      if (frame % pdcp_period == 0) {
#ifdef PAD_FINE
        pad_inject_job(1, eNB_id, frame, next_slot, last_slot, JT_PDCP, oai_emulation.info.time_ms);
#else
        pdcp_run(frame, 1, 0, eNB_id);//PHY_vars_eNB_g[eNB_id]->Mod_id
#endif
      }

      //Phy/Mac layer
#ifdef PAD_FINE
      pad_inject_job(1, eNB_id, frame, next_slot, last_slot, JT_PHY_MAC, oai_emulation.info.time_ms);
#else
      phy_procedures_eNB_lte (last_slot, next_slot, PHY_vars_eNB_g[eNB_id], abstraction_flag, no_relay, NULL);
#endif

#ifdef PRINT_STATS

      if(last_slot==9 && frame%10==0)
        if(eNB_avg_thr)
          fprintf(eNB_avg_thr,"%d %d\n",PHY_vars_eNB_g[eNB_id]->frame,(PHY_vars_eNB_g[eNB_id]->total_system_throughput)/((PHY_vars_eNB_g[eNB_id]->frame+1)*10));

      if (eNB_stats) {
        len = dump_eNB_stats(PHY_vars_eNB_g[eNB_id], stats_buffer, 0);
        rewind (eNB_stats);
        fwrite (stats_buffer, 1, len, eNB_stats);
        fflush(eNB_stats);
      }

#ifdef OPENAIR2

      if (eNB_l2_stats) {
        len = dump_eNB_l2_stats (stats_buffer, 0);
        rewind (eNB_l2_stats);
        fwrite (stats_buffer, 1, len, eNB_l2_stats);
        fflush(eNB_l2_stats);
      }

#endif
#endif
    }

#ifdef PAD_SYNC

    if ((direction == SF_DL) || ((direction == SF_S) && (next_slot%2==0)) )
      pad_synchronize();

#endif

    // Call ETHERNET emulation here
    //emu_transport (frame, last_slot, next_slot, direction, oai_emulation.info.frame_type, ethernet_flag);

    if ((next_slot % 2) == 0)
      clear_UE_transport_info (oai_emulation.info.nb_ue_local);

    for (UE_id = oai_emulation.info.first_ue_local;
         (UE_id < (oai_emulation.info.first_ue_local+oai_emulation.info.nb_ue_local)) && (oai_emulation.info.cli_start_ue[UE_id]==1);
         UE_id++)
      if (frame >= (UE_id * 20)) {    // activate UE only after 20*UE_id frames so that different UEs turn on separately

        LOG_D(EMU,"PHY procedures UE %d for frame %d, slot %d (subframe TX %d, RX %d)\n",
              UE_id, frame, slot, next_slot >> 1,last_slot>>1);

        if (PHY_vars_UE_g[UE_id]->UE_mode[0] != NOT_SYNCHED) {
          if (frame>0) {
            PHY_vars_UE_g[UE_id]->frame = frame;

            //Application UE
#ifdef PAD_FINE
            pad_inject_job(0, UE_id, frame, next_slot, last_slot, JT_OTG, oai_emulation.info.time_ms);
#else
            update_otg_UE(UE_id + NB_eNB_INST, oai_emulation.info.time_ms);
#endif

            //Access layer UE
            if (frame % pdcp_period == 0) {
#ifdef PAD_FINE
              pad_inject_job(0, UE_id, frame, next_slot, last_slot, JT_PDCP, oai_emulation.info.time_ms);
#else
              pdcp_run(frame, 0, UE_id, 0);
#endif
            }

            //Phy/Mac layer UE
#ifdef PAD_FINE
            pad_inject_job(0, UE_id, frame, next_slot, last_slot, JT_PHY_MAC, oai_emulation.info.time_ms);
#else
            phy_procedures_UE_lte (last_slot, next_slot, PHY_vars_UE_g[UE_id], 0, abstraction_flag, normal_txrx, no_relay, NULL);
            ue_data[UE_id]->tx_power_dBm = PHY_vars_UE_g[UE_id]->tx_power_dBm;
#endif
          }
        } else {
          if (abstraction_flag==1) {
            LOG_E(EMU, "sync not supported in abstraction mode (UE%d,mode%d)\n", UE_id, PHY_vars_UE_g[UE_id]->UE_mode[0]);
            exit(-1);
          }

          if ((frame>0) && (last_slot == (LTE_SLOTS_PER_FRAME-2))) {
#ifdef PAD_FINE
            pad_inject_job(0, UE_id, frame, next_slot, last_slot, JT_INIT_SYNC, oai_emulation.info.time_ms);
#else
            initial_sync(PHY_vars_UE_g[UE_id],normal_txrx);
#endif
            /* LONG write output comment DELETED here */
          }
        }

#ifdef PRINT_STATS

        if(last_slot==2 && frame%10==0)
          if (UE_stats_th[UE_id])
            fprintf(UE_stats_th[UE_id],"%d %d\n",frame, PHY_vars_UE_g[UE_id]->bitrate[0]/1000);

        if (UE_stats[UE_id]) {
          len = dump_ue_stats (PHY_vars_UE_g[UE_id], stats_buffer, 0, normal_txrx, 0);
          rewind (UE_stats[UE_id]);
          fwrite (stats_buffer, 1, len, UE_stats[UE_id]);
          fflush(UE_stats[UE_id]);
        }

#endif
      }

#ifdef PAD_SYNC

    if ((direction == SF_UL) || ((direction == SF_S) && (next_slot%2==1)) )
      pad_synchronize();

#endif

    emu_transport (frame, last_slot, next_slot,direction, oai_emulation.info.frame_type, ethernet_flag);

    if ((direction  == SF_DL)|| (frame_parms->frame_type==0)) {
      for (UE_id=0; UE_id<NB_UE_INST; UE_id++) {

#ifdef PAD
        pad_inject_job(0, UE_id, frame, next_slot, last_slot, JT_DL, oai_emulation.info.time_ms);
#else
        do_DL_sig(r_re0,r_im0,r_re,r_im,s_re,s_im,eNB2UE,enb_data,ue_data,next_slot,abstraction_flag,frame_parms,UE_id);
#endif
      }
    }

    if ((direction  == SF_UL)|| (frame_parms->frame_type==0)) { //if ((subframe<2) || (subframe>4))
      do_UL_sig(r_re0,r_im0,r_re,r_im,s_re,s_im,UE2eNB,enb_data,ue_data,next_slot,abstraction_flag,frame_parms,frame);

      /*
            int ccc;
            fprintf(SINRpost,"SINRdb For eNB New Subframe : \n ");
            for(ccc = 0 ; ccc<301; ccc++)
            {
              fprintf(SINRpost,"_ %f ", SINRpost_eff[ccc]);
            }
            fprintf(SINRpost,"SINRdb For eNB : %f \n ", SINRpost_eff[ccc]);
            */
    }

    if ((direction == SF_S)) {//it must be a special subframe
      if (next_slot%2==0) {//DL part
        for (UE_id=0; UE_id<NB_UE_INST; UE_id++) {
#ifdef PAD
          pad_inject_job(0, UE_id, frame, next_slot, last_slot, JT_DL, oai_emulation.info.time_ms);
#else
          do_DL_sig(r_re0,r_im0,r_re,r_im,s_re,s_im,eNB2UE,enb_data,ue_data,next_slot,abstraction_flag,frame_parms,UE_id);
#endif
        }

        /*
              for (aarx=0;aarx<UE2eNB[1][0]->nb_rx;aarx++)
              for (aatx=0;aatx<UE2eNB[1][0]->nb_tx;aatx++)
              for (k=0;k<UE2eNB[1][0]->channel_length;k++)
              printf("SB(%d,%d,%d)->(%f,%f)\n",k,aarx,aatx,UE2eNB[1][0]->ch[aarx+(aatx*UE2eNB[1][0]->nb_rx)][k].r,UE2eNB[1][0]->ch[aarx+(aatx*UE2eNB[1][0]->nb_rx)][k].i);
            */
      } else { // UL part
        /*#ifdef PAD
        pthread_mutex_lock(&(pool->sync_lock));
        while(pool->active != 0) {
        pthread_cond_wait(&(pool->sync_notify), &(pool->sync_lock));
        }
        pthread_mutex_unlock(&(pool->sync_lock));
        #endif*/
        do_UL_sig(r_re0,r_im0,r_re,r_im,s_re,s_im,UE2eNB,enb_data,ue_data,next_slot,abstraction_flag,frame_parms,frame);
        /*
                int ccc;
                fprintf(SINRpost,"SINRdb For eNB New Subframe : \n ");
                for(ccc = 0 ; ccc<301; ccc++)
                {
                  fprintf(SINRpost,"_ %f ", SINRpost_eff[ccc]);
                }
                fprintf(SINRpost,"SINRdb For eNB : %f \n ", SINRpost_eff[ccc]);
                */
      }
    }

    if ((last_slot == 1) && (frame == 0)
        && (abstraction_flag == 0) && (oai_emulation.info.n_frames == 1)) {

      write_output ("dlchan0.m", "dlch0",
                    &(PHY_vars_UE_g[0]->lte_ue_common_vars.common_vars_rx_data_per_thread[subframe&0x1].dl_ch_estimates[0][0][0]),
                    (6 * (PHY_vars_UE_g[0]->lte_frame_parms.ofdm_symbol_size)), 1, 1);
      write_output ("dlchan1.m", "dlch1",
                    &(PHY_vars_UE_g[0]->lte_ue_common_vars.common_vars_rx_data_per_thread[subframe&0x1].dl_ch_estimates[1][0][0]),
                    (6 * (PHY_vars_UE_g[0]->lte_frame_parms.ofdm_symbol_size)), 1, 1);
      write_output ("dlchan2.m", "dlch2",
                    &(PHY_vars_UE_g[0]->lte_ue_common_vars.common_vars_rx_data_per_thread[subframe&0x1].dl_ch_estimates[2][0][0]),
                    (6 * (PHY_vars_UE_g[0]->lte_frame_parms.ofdm_symbol_size)), 1, 1);
      write_output ("pbch_rxF_comp0.m", "pbch_comp0",
                    PHY_vars_UE_g[0]->lte_ue_pbch_vars[0]->rxdataF_comp[0], 6 * 12 * 4, 1, 1);
      write_output ("pbch_rxF_llr.m", "pbch_llr",
                    PHY_vars_UE_g[0]->lte_ue_pbch_vars[0]->llr, (frame_parms->Ncp == 0) ? 1920 : 1728, 1, 4);
    }

    if (next_slot %2 == 0) {
      clock_gettime (CLOCK_REALTIME, &time_spec);
      time_last = time_now;
      time_now = (unsigned long) time_spec.tv_nsec;
      td = (int) (time_now - time_last);

      if (td>0) {
        td_avg = (int)(((K*(long)td) + (((1<<3)-K)*((long)td_avg)))>>3); // in us
        LOG_T(EMU,"sleep frame %d, time_now %ldus,time_last %ldus,average time difference %ldns, CURRENT TIME DIFF %dus, avgerage difference from the target %dus\n",
              frame, time_now,time_last,td_avg, td/1000,(td_avg-TARGET_SF_TIME_NS)/1000);
      }

      if (td_avg<(TARGET_SF_TIME_NS - SF_DEVIATION_OFFSET_NS)) {
        sleep_time_us += SLEEP_STEP_US;
        LOG_D(EMU,"Faster than realtime increase the avg sleep time for %d us, frame %d\n",
              sleep_time_us,frame);
        // LOG_D(EMU,"Faster than realtime increase the avg sleep time for %d us, frame %d, time_now %ldus,time_last %ldus,average time difference %ldns, CURRENT TIME DIFF %dus, avgerage difference from the target %dus\n",    sleep_time_us,frame, time_now,time_last,td_avg, td/1000,(td_avg-TARGET_SF_TIME_NS)/1000);
      } else if (td_avg > (TARGET_SF_TIME_NS + SF_DEVIATION_OFFSET_NS)) {
        sleep_time_us-= SLEEP_STEP_US;
        LOG_D(EMU,"Slower than realtime reduce the avg sleep time for %d us, frame %d, time_now\n",
              sleep_time_us,frame);
        //LOG_T(EMU,"Slower than realtime reduce the avg sleep time for %d us, frame %d, time_now %ldus,time_last %ldus,average time difference %ldns, CURRENT TIME DIFF %dus, avgerage difference from the target %dus\n",     sleep_time_us,frame, time_now,time_last,td_avg, td/1000,(td_avg-TARGET_SF_TIME_NS)/1000);
      }
    } // end if next_slot%2

    slot++;

    if (slot == 20) { //Frame's Epilogue
      frame++;
      slot = 0;

      // if n_frames not set by the user or is greater than max num frame then set adjust the frame counter
      if ( (oai_emulation.info.n_frames_flag == 0) || (oai_emulation.info.n_frames >= 0xffff) ) {
        frame %=(oai_emulation.info.n_frames-1);
      }

      oai_emulation.info.time_s += 0.01;

      if ((frame>=1)&&(frame<=9)&&(abstraction_flag==0)) {
        write_output("UEtxsig0.m","txs0", PHY_vars_UE_g[0]->lte_ue_common_vars.txdata[0],FRAME_LENGTH_COMPLEX_SAMPLES,1,1);
        sprintf(fname,"eNBtxsig%d.m",frame);
        sprintf(vname,"txs%d",frame);
        write_output(fname,vname, PHY_vars_eNB_g[0]->lte_eNB_common_vars.txdata[0][0],FRAME_LENGTH_COMPLEX_SAMPLES,1,1);
        write_output("eNBtxsigF0.m","txsF0",PHY_vars_eNB_g[0]->lte_eNB_common_vars.txdataF[0][0],PHY_vars_eNB_g[0]->lte_frame_parms.symbols_per_tti*PHY_vars_eNB_g[0]->lte_frame_parms.ofdm_symbol_size,1,1);

        write_output("UErxsig0.m","rxs0", PHY_vars_UE_g[0]->lte_ue_common_vars.rxdata[0],FRAME_LENGTH_COMPLEX_SAMPLES,1,1);
        write_output("eNBrxsig0.m","rxs0", PHY_vars_eNB_g[0]->lte_eNB_common_vars.rxdata[0][0],FRAME_LENGTH_COMPLEX_SAMPLES,1,1);
      }

#ifdef XFORMS
      do_xforms();
#endif

      // calibrate at the end of each frame if there is some time  left
      if((sleep_time_us > 0)&& (ethernet_flag ==0)) {
        LOG_I(EMU,"[TIMING] Adjust average frame duration, sleep for %d us\n",sleep_time_us);
        usleep(sleep_time_us);
        sleep_time_us=0; // reset the timer, could be done per n SF
      }

#ifdef SMBV

      if ((frame == config_frames[0]) || (frame == config_frames[1]) || (frame == config_frames[2]) || (frame == config_frames[3])) {
        smbv_frame_cnt++;
      }

#endif
    }
  }

  t = clock() - t;
  printf("rrc Duration of the simulation: %f seconds\n",((float)t)/CLOCKS_PER_SEC);

  fclose(SINRpost);
  LOG_I(EMU,">>>>>>>>>>>>>>>>>>>>>>>>>>> OAIEMU Ending <<<<<<<<<<<<<<<<<<<<<<<<<<\n\n");

  free(otg_pdcp_buffer);

#ifdef SMBV

  if (config_smbv) {
    smbv_send_config (smbv_fname,smbv_ip);
  }

#endif

  //Perform KPI measurements
  if (oai_emulation.info.otg_enabled==1)
    kpi_gen();

#ifdef PAD
  pad_finalize();
#endif

  // relase all rx state
  if (ethernet_flag == 1) {
    emu_transport_release ();
  }

  if (abstraction_flag == 0) {
    /*
           #ifdef IFFT_FPGA
           free(txdataF2[0]);
           free(txdataF2[1]);
           free(txdataF2);
           free(txdata[0]);
           free(txdata[1]);
           free(txdata);
           #endif
         */

    for (i = 0; i < 2; i++) {
      free (s_re[i]);
      free (s_im[i]);
      free (r_re[i]);
      free (r_im[i]);
    }

    free (s_re);
    free (s_im);
    free (r_re);
    free (r_im);
    lte_sync_time_free ();
  }

  //  pthread_join(sigth, NULL);

  // added for PHY abstraction
  if (oai_emulation.info.ocm_enabled == 1) {
    for (eNB_id = 0; eNB_id < NUMBER_OF_eNB_MAX; eNB_id++)
      free(enb_data[eNB_id]);

    for (UE_id = 0; UE_id < NUMBER_OF_UE_MAX; UE_id++)
      free(ue_data[UE_id]);
  } //End of PHY abstraction changes

#ifdef OPENAIR2
  mac_top_cleanup();
#endif

#ifdef PRINT_STATS

  for(UE_id=0; UE_id<NB_UE_INST; UE_id++) {
    if (UE_stats[UE_id])
      fclose (UE_stats[UE_id]);

    if(UE_stats_th[UE_id])
      fclose (UE_stats_th[UE_id]);
  }

  if (eNB_stats)
    fclose (eNB_stats);

  if (eNB_avg_thr)
    fclose (eNB_avg_thr);

  if (eNB_l2_stats)
    fclose (eNB_l2_stats);

#endif

  // stop OMG
  stop_mobility_generator(oai_emulation.info.omg_model_ue);//omg_param_list.mobility_type
#ifdef OPENAIR2

  if (oai_emulation.info.omv_enabled == 1)
    omv_end(pfd[1],omv_data);

#endif

  if ((oai_emulation.info.ocm_enabled == 1) && (ethernet_flag == 0) && (ShaF != NULL))
    destroyMat(ShaF,map1, map2);

  if (opt_enabled == 1 )
    terminate_opt();

  if (oai_emulation.info.cli_enabled)
    cli_server_cleanup();

  //bring oai if down
  terminate();
  logClean();
  VCD_SIGNAL_DUMPER_CLOSE();
  //printf("FOR MAIN TIMES = %d &&&& OTG TIMES = %d <-> FOR TIMES = %d <-> IF TIMES = %d\n", for_main_times, otg_times, for_times, if_times);

}
//<<PAD>>//

void terminate(void)
{
  int i;
  char interfaceName[8];

  for (i=0; i < NUMBER_OF_eNB_MAX+NUMBER_OF_UE_MAX; i++)
    if (oai_emulation.info.oai_ifup[i]==1) {
      sprintf(interfaceName, "oai%d", i);
      bringInterfaceUp(interfaceName,0);
    }
}

#ifdef OPENAIR2
int omv_write (int pfd,  Node_list enb_node_list, Node_list ue_node_list, Data_Flow_Unit omv_data)
{
  int i,j;
  omv_data.end=0;

  //omv_data.total_num_nodes = NB_UE_INST + NB_eNB_INST;
  for (i=0; i<NB_eNB_INST; i++) {
    if (enb_node_list != NULL) {
      omv_data.geo[i].x = (enb_node_list->node->X_pos < 0.0)? 0.0 : enb_node_list->node->X_pos;
      omv_data.geo[i].y = (enb_node_list->node->Y_pos < 0.0)? 0.0 : enb_node_list->node->Y_pos;
      omv_data.geo[i].z = 1.0;
      omv_data.geo[i].mobility_type = oai_emulation.info.omg_model_enb;
      omv_data.geo[i].node_type = 0; //eNB
      enb_node_list = enb_node_list->next;
      omv_data.geo[i].Neighbors=0;

      for (j=NB_eNB_INST; j< NB_UE_INST + NB_eNB_INST ; j++) {
        if (is_UE_active(i,j - NB_eNB_INST ) == 1) {
          omv_data.geo[i].Neighbor[omv_data.geo[i].Neighbors]=  j;
          omv_data.geo[i].Neighbors++;
          LOG_D(OMG,"[eNB %d][UE %d] is_UE_active(i,j) %d geo (x%d, y%d) num neighbors %d\n", i,j-NB_eNB_INST, is_UE_active(i,j-NB_eNB_INST),
                omv_data.geo[i].x, omv_data.geo[i].y, omv_data.geo[i].Neighbors);
        }
      }
    }
  }

  for (i=NB_eNB_INST; i<NB_UE_INST+NB_eNB_INST; i++) {
    if (ue_node_list != NULL) {
      omv_data.geo[i].x = (ue_node_list->node->X_pos < 0.0) ? 0.0 : ue_node_list->node->X_pos;
      omv_data.geo[i].y = (ue_node_list->node->Y_pos < 0.0) ? 0.0 : ue_node_list->node->Y_pos;
      omv_data.geo[i].z = 1.0;
      omv_data.geo[i].mobility_type = oai_emulation.info.omg_model_ue;
      omv_data.geo[i].node_type = 1; //UE
      //trial
      omv_data.geo[i].state = 1;
      omv_data.geo[i].rnti = 88;
      omv_data.geo[i].connected_eNB = 0;
      omv_data.geo[i].RSRP = 66;
      omv_data.geo[i].RSRQ = 55;
      omv_data.geo[i].Pathloss = 44;
      omv_data.geo[i].RSSI[0] = 33;
      omv_data.geo[i].RSSI[1] = 22;
      omv_data.geo[i].RSSI[2] = 11;

      ue_node_list = ue_node_list->next;
      omv_data.geo[i].Neighbors=0;

      for (j=0; j< NB_eNB_INST ; j++) {
        if (is_UE_active(j,i-NB_eNB_INST) == 1) {
          omv_data.geo[i].Neighbor[ omv_data.geo[i].Neighbors]=j;
          omv_data.geo[i].Neighbors++;
          LOG_D(OMG,"[UE %d][eNB %d] is_UE_active  %d geo (x%d, y%d) num neighbors %d\n", i-NB_eNB_INST,j, is_UE_active(j,i-NB_eNB_INST),
                omv_data.geo[i].x, omv_data.geo[i].y, omv_data.geo[i].Neighbors);
        }
      }
    }
  }

  if( write( pfd, &omv_data, sizeof(struct Data_Flow_Unit) ) == -1 )
    perror( "write omv failed" );

  return 1;
}

void omv_end (int pfd, Data_Flow_Unit omv_data)
{
  omv_data.end=1;

  if( write( pfd, &omv_data, sizeof(struct Data_Flow_Unit) ) == -1 )
    perror( "write omv failed" );
}
#endif


/* sum/gsl_sum.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#ifndef __GSL_SUM_H__
#define __GSL_SUM_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS          /* empty */
# define __END_DECLS            /* empty */
#endif

__BEGIN_DECLS

/*  Workspace for Levin U Transform with error estimation,
 *
 *   size        = number of terms the workspace can handle
 *   sum_plain   = simple sum of series
 *   q_num       = backward diagonal of numerator; length = size
 *   q_den       = backward diagonal of denominator; length = size
 *   dq_num      = table of numerator derivatives; length = size**2
 *   dq_den      = table of denominator derivatives; length = size**2
 *   dsum        = derivative of sum wrt term i; length = size
 */

typedef struct
{
  size_t size;
  size_t i;                     /* position in array */
  size_t terms_used;            /* number of calls */
  double sum_plain;
  double *q_num;
  double *q_den;
  double *dq_num;
  double *dq_den;
  double *dsum;
}
gsl_sum_levin_u_workspace;

GSL_EXPORT gsl_sum_levin_u_workspace *gsl_sum_levin_u_alloc (size_t n);
GSL_EXPORT void gsl_sum_levin_u_free (gsl_sum_levin_u_workspace * w);

/* Basic Levin-u acceleration method.
 *
 *   array       = array of series elements
 *   n           = size of array
 *   sum_accel   = result of summation acceleration
 *   err         = estimated error
 *
 * See [Fessler et al., ACM TOMS 9, 346 (1983) and TOMS-602]
 */

GSL_EXPORT int gsl_sum_levin_u_accel (const double *array,
                                      const size_t n,
                                      gsl_sum_levin_u_workspace * w,
                                      double *sum_accel, double *abserr);

/* Basic Levin-u acceleration method with constraints on the terms
 * used,
 *
 *   array       = array of series elements
 *   n           = size of array
 *   min_terms   = minimum number of terms to sum
 *   max_terms   = maximum number of terms to sum
 *   sum_accel   = result of summation acceleration
 *   err         = estimated error
 *
 * See [Fessler et al., ACM TOMS 9, 346 (1983) and TOMS-602]
 */

GSL_EXPORT int gsl_sum_levin_u_minmax (const double *array,
                                       const size_t n,
                                       const size_t min_terms,
                                       const size_t max_terms,
                                       gsl_sum_levin_u_workspace * w,
                                       double *sum_accel, double *abserr);

/* Basic Levin-u step w/o reference to the array of terms.
 * We only need to specify the value of the current term
 * to execute the step. See TOMS-745.
 *
 * sum = t0 + ... + t_{n-1} + term;  term = t_{n}
 *
 *   term   = value of the series term to be added
 *   n      = position of term in series (starting from 0)
 *   sum_accel = result of summation acceleration
 *   sum_plain = simple sum of series
 */

GSL_EXPORT
int
gsl_sum_levin_u_step (const double term,
                      const size_t n,
                      const size_t nmax,
                      gsl_sum_levin_u_workspace * w,
                      double *sum_accel);

/* The following functions perform the same calculation without
   estimating the errors. They require O(N) storage instead of O(N^2).
   This may be useful for summing many similar series where the size
   of the error has already been estimated reliably and is not
   expected to change.  */

typedef struct
{
  size_t size;
  size_t i;                     /* position in array */
  size_t terms_used;            /* number of calls */
  double sum_plain;
  double *q_num;
  double *q_den;
  double *dsum;
}
gsl_sum_levin_utrunc_workspace;

GSL_EXPORT gsl_sum_levin_utrunc_workspace *gsl_sum_levin_utrunc_alloc (size_t n);
GSL_EXPORT void gsl_sum_levin_utrunc_free (gsl_sum_levin_utrunc_workspace * w);

GSL_EXPORT int gsl_sum_levin_utrunc_accel (const double *array,
                                           const size_t n,
                                           gsl_sum_levin_utrunc_workspace * w,
                                           double *sum_accel, double *abserr_trunc);

GSL_EXPORT int gsl_sum_levin_utrunc_minmax (const double *array,
                                            const size_t n,
                                            const size_t min_terms,
                                            const size_t max_terms,
                                            gsl_sum_levin_utrunc_workspace * w,
                                            double *sum_accel, double *abserr_trunc);

GSL_EXPORT int gsl_sum_levin_utrunc_step (const double term,
                                          const size_t n,
                                          gsl_sum_levin_utrunc_workspace * w,
                                          double *sum_accel);

__END_DECLS

#endif /* __GSL_SUM_H__ */


/**
 * author: Jochen K"upper
 * created: Jan 2002
 * file: pygsl/src/statistics/longmodule.c
 * $Id: ucharmodule.c,v 1.5 2004/03/24 08:40:45 schnizer Exp $
 *
 *"
 */

#include <Python.h>
#include <pygsl/error_helpers.h>
#include <pygsl/block_helpers.h>
#include <gsl/gsl_statistics.h>

/* include real functions for different data-types */

#define STATMOD_APPEND_PY_TYPE(X) X ## Int
#define STATMOD_APPEND_PYC_TYPE(X) X ## UBYTE
#define STATMOD_FUNC_EXT(X, Y) X ## _uchar ## Y
#define STATMOD_PY_AS_C PyInt_AsLong
#define STATMOD_C_TYPE unsigned char
#include "functions.c"

PyGSL_STATISTICS_INIT(uchar, "uchar")

/*
 * Local Variables:
 * mode: c
 * c-file-style: "Stroustrup"
 * End:
 */


#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/block/gsl_block.h>
#include <gsl/vector/gsl_vector.h>

#define BASE_DOUBLE
#include <gsl/templates_on.h>
#include <gsl/vector/file_source.c>
#include <gsl/templates_off.h>
#undef BASE_DOUBLE


/**
 * author: Jochen K"upper
 * created: Jan 2002
 * file: pygsl/src/statisticsmodule.c
 * $Id: doublemodule.c,v 1.9 2004/03/24 08:40:45 schnizer Exp $
 *
 * optional usage of Numeric module, available at http://numpy.sourceforge.net
 * "
 */

#include <pygsl/error_helpers.h>
#include <pygsl/block_helpers.h>
#include <Python.h>
#include <gsl/gsl_statistics.h>

/* include real functions for default data-types (double in C) */

#define STATMOD_WEIGHTED
#define STATMOD_APPEND_PY_TYPE(X) X ## Float
#define STATMOD_APPEND_PYC_TYPE(X) X ## DOUBLE
#define STATMOD_FUNC_EXT(X, Y) X ## Y
#define STATMOD_PY_AS_C PyFloat_AsDouble
#define STATMOD_C_TYPE double
#define PyGSL_STATISTICS_IMPORT_API
#include "functions.c"

PyGSL_STATISTICS_INIT(double, "double")

/*
 * Local Variables:
 * mode: c
 * c-file-style: "Stroustrup"
 * End:
 */


/**
 *
 * @file core_sgeqp3_init.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mark Gates
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:44:49 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_float
 *
 *  CORE_sgeqp3_init initializes jpvt to [1, ..., n].
 *  Uses 1-based indexing for Fortran compatability.
 *
 *******************************************************************************
 *
 *  @param[in] n
 *          Size of vector.
 *
 *  @param[in,out] jpvt
 *          Vector of size n.
 *          On exit, jpvt[i] = i+1.
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgeqp3_init = PCORE_sgeqp3_init
#define CORE_sgeqp3_init PCORE_sgeqp3_init
#endif
void CORE_sgeqp3_init( int n, int *jpvt )
{
    int j;
    for( j = 0; j < n; ++j ) {
        jpvt[j] = j+1;
    }
}


#include "../include/paralleltt.h"
#include <math.h>

#include <stdlib.h>
#include <stdio.h>
#include <mpi.h>
#include <cblas.h>
#include <string.h>
#include <lapacke.h>

#define min(a,b) ((a)>(b)?(b):(a))
#define abs(a) ((a)>(0)?(a):(-a))

#ifndef HEAD
#define HEAD (int) 0
#endif

matrix_tt* matrix_tt_init(const int m, const int n)
{
    matrix_tt* A = (matrix_tt*) malloc(sizeof(matrix_tt));
    A->m = m;
    A->n = n;
    A->transpose = 0;
    A->offset = 0;
    A->lda = m;
    A->X_size = (long) m*n;
    A->X = (double*) malloc(A->X_size*sizeof(double));

    return A;
}

void matrix_tt_wrap_update(matrix_tt* A, int m, int n, double* X)
{
    A->m = m;
    A->n = n;
    A->transpose = 0;
    A->offset = 0;
    A->lda = m;
    A->X_size = (long) m*n;
    A->X = X;
}

matrix_tt* matrix_tt_wrap(int m, int n, double* X)
{
    matrix_tt* A = (matrix_tt*) malloc(sizeof(matrix_tt));
    A->m = m;
    A->n = n;
    A->transpose = 0;
    A->offset = 0;
    A->lda = m;
    A->X_size = (long) m*n;
    A->X = X;
    return A;
}

matrix_tt* matrix_tt_copy(const matrix_tt* A)
{
    matrix_tt* B = (matrix_tt*) malloc(sizeof(matrix_tt));
    int m = A->m; int n = A->n; int X_size = A->X_size;
    B->m = m;
    B->n = n;
    B->transpose = A->transpose;
    B->offset = A->offset;
    B->lda = A->lda;
    B->X_size = X_size;
    B->X = (double*) malloc(X_size * sizeof(double));
    memcpy(B->X, A->X, X_size * sizeof(double));

    return B;
}

// Copy from matrix A to matrix B
void matrix_tt_copy_data(matrix_tt* B, const matrix_tt* A)
{
    int BT = (B->transpose == 0) ? 0 : 1;
    int AT = (A->transpose == 0) ? 0 : 1;
    if (BT == AT){
        if ((B->m != A->m) || (B->n != A->n)){
            printf("matrix_tt_copy_data: B (%d x %d) is not the same size as A (%d x %d)\n", B->n, B->m, A->n, A->m);
            return;
        }
        for (int jj = 0; jj < B->n; ++jj){
            memcpy((B->X) + (B->offset) + (B->lda)*jj, (A->X) + (A->offset) + jj*(A->lda), (B->m)*sizeof(double));
        }
    }
    else{
        if ((B->m != A->n) || (B->n != A->m)){
            printf("matrix_tt_copy_data: B (%d x %d, transpose = %d) is not the same size as A (%d x %d, transpose = %d)\n",
                    B->n, B->m, B->transpose, A->n, A->m, A->transpose);
            return;
        }
        for (int jj = 0; jj < B->n; ++jj){
            double* BX = (B->X) + (B->offset) + (B->lda)*jj;
            double* AX = (A->X) + (A->offset) + jj;
            for (int ii = 0; ii < B->m; ++ii){
                BX[ii] = AX[ii*(A->lda)];
            }
        }
    }
}

matrix_tt* submatrix_copy(const matrix_tt* A)
{
    matrix_tt* B = (matrix_tt*) malloc(sizeof(matrix_tt));
    int m = A->m; int n = A->n; int X_size = m*n;
    B->m = m;
    B->n = n;
    B->transpose = A->transpose;
    B->offset = 0;
    B->lda = m;
    B->X_size = X_size;
    B->X = (double*) malloc(X_size * sizeof(double));
    matrix_tt_copy_data(B, A);

    return(B);
}

// NOTE: reshape and submatrix will not work well together.
void matrix_tt_reshape(int m, int n, matrix_tt* A)
{
    long new_size = (long) m*n;
    if (new_size > A->X_size){
        printf("Reshape failed: m*n=%ld is too large for X_size=%ld\n", (long) new_size, A->X_size);
    }
    else{
        A->m = m;
        A->n = n;
        A->offset = 0;
        A->lda = m;
    }
}

// In matlab notation, returns A[ii1:ii2-1, jj1:jj2-1]
matrix_tt* submatrix(const matrix_tt* A, int ii1, int ii2, int jj1, int jj2)
{
    int mA = A->m; int nA = A->n; int lda = A->lda;
    if ((ii1 < 0) || (ii2 < ii1) || (mA < ii2) || (jj1 < 0) || (jj2 < jj1) || (nA < jj2)){
        printf("Cannot take the A[%d:%d,%d:%d] of a %d by %d matrix (Matlab notation)\n", ii1, ii2-1, jj1, jj2-1, mA, nA);
    }
    matrix_tt* B = (matrix_tt*) malloc(sizeof(matrix_tt));

    B->m = ii2 - ii1;
    B->n = jj2 - jj1;
    B->transpose = A->transpose;
    B->offset = ii1 + jj1*lda;
    B->lda = lda;
    B->X_size = A->X_size;
    B->X = A->X + A->offset;

    return B;
}

// Updates the indices of the submatrix A
void submatrix_update(matrix_tt* A, int ii1, int ii2, int jj1, int jj2)
{
    A->m = ii2 - ii1;
    A->n = jj2 - jj1;
    A->offset = ii1 + jj1*(A->lda);
}

// Get an element of the matrix
double matrix_tt_element(const matrix_tt* A, int ii, int jj)
{
    return A->X[A->offset + ii + (A->lda)*jj];
}

void matrix_tt_free(matrix_tt* A)
{
    free(A->X); A->X = NULL;
    free(A);
    return;
}

void matrix_tt_print(const matrix_tt* A, int just_the_matrix)
{
    int m = A->m; int n = A->n; int T = A->transpose;
    int offset = A->offset; int lda = A->lda;

    if (!just_the_matrix){
        printf("Size of matrix: m=%d, n=%d\n", m, n);
        printf("Transpose=%d, offset=%d, lda=%d\n", T, offset, lda);
        printf("X_size=%ld\n", A->X_size);
        printf("X");
        if (T){ printf("^T"); }
        printf(" is \n");
    }

    int mloop = T ? m : n;
    int nloop = T ? n : m;
    printf("[");
    for (int jj = 0; jj < nloop; ++jj){
        for (int ii = 0; ii < mloop; ++ii){
            if (!T){ printf("%f",A->X[lda*ii + jj + offset]); }
            else{    printf("%f",A->X[lda*jj + ii + offset]); }
            if (ii < mloop-1){ printf(", "); }
        }

        if (jj == nloop - 1){
            printf("]\n");
        }
        else{
            printf(",\n ");
        }
    }
}

double frobenius_norm(matrix_tt* A){
    double norm = 0;
    if (A->m == A->lda){
        norm = cblas_dnrm2(A->m * A->n, A->offset + A->X, 1);
    }
    else{
        for (int ii = 0; ii < A->n; ++ii){
            double tmp = cblas_dnrm2(A->m, A->offset + (A->lda)*ii + A->X, 1);
            norm = sqrt(norm*norm + tmp*tmp);
        }
    }
    return norm;
}

// Performs the operation C = alpha*A*B + beta*C, where A and B are appropriately transposed
int matrix_tt_dgemm(const matrix_tt* A, const matrix_tt* B, matrix_tt* C, const double alpha, const double beta)
{
    int mA = A->m; int nA = A->n;
    int mB = B->m; int nB = B->n;
    int m_check = C->m; int n_check = C->n; int k_check;

    int m; int n; int k;
    CBLAS_TRANSPOSE TransA;
    CBLAS_TRANSPOSE TransB;
    if (A->transpose == 0){
        TransA = CblasNoTrans;
        m = mA;
        k = nA;
    }
    else{
        TransA = CblasTrans;
        m = nA;
        k = mA;
    }

    if (B->transpose == 0){
        TransB = CblasNoTrans;
        k_check = mB;
        n = nB;
    }
    else{
        TransB = CblasTrans;
        k_check = nB;
        n = mB;
    }

    if ((m != m_check) || (n != n_check) || (k != k_check)){
        printf("Dimensions of input arrays do not match:\n");
        printf("A: m=%d, n=%d, tranpose=%d\n",mA,nA,A->transpose);
        printf("B: m=%d, n=%d, tranpose=%d\n",mB,nB,B->transpose);
        printf("C: m=%d, n=%d, tranpose=%d\n",m_check,n_check,C->transpose);
        return 1;
    }

    C->transpose = 0;

    cblas_dgemm(CblasColMajor, TransA, TransB,
                m, n, k,
                alpha,
                A->X + A->offset, A->lda,
                B->X + B->offset, B->lda,
                beta,
                C->X + C->offset, C->lda);

    return 0;
}

// Solves the least squares problem Ax = b
// At the end, it just copies the result into x. This probably isn't the most efficient way to do this
void matrix_tt_dgels(matrix_tt* x, matrix_tt* A, const matrix_tt* b)
{
    if (x->transpose != 0){
        printf("matrix_tt_least_squares: x cannot be transposed for dgels\n");
        return;
    }

    if (b->transpose != 0){
        printf("matrix_tt_least_squares: b cannot be transposed for dgels\n");
        return;
    }

    int nrhs = x->n;
    char TransA;
    int m_check;
    int n_check;

    if (A->transpose == 0){
        TransA = 'N';
        m_check = A->m;
        n_check = A->n;
    }
    else{
        TransA = 'T';
        m_check = A->n;
        n_check = A->m;
    }

    if (n_check != x->m){
        printf("right index of A (%d) does not match left index of x (%d)\n", n_check, x->m);
    }
    if (m_check != b->m){
        printf("left index of A (%d) does not match left index of b (%d)\n", m_check, b->m);
    }
    if (x->n != b->n){
        printf("right index of x (%d) does not match right index of b (%d)\n", x->n, b->n);
    }

    int m = A->m;
    int n = A->n;

    int info = LAPACKE_dgels(LAPACK_COL_MAJOR, TransA, m, n, nrhs, A->X + A->offset, A->lda, b->X + b->offset, b->lda);

    matrix_tt* result = submatrix(b, 0, x->m, 0, x->n);
    matrix_tt_copy_data(x, result);
    free(result);
}

// Input:
//   Q - the matrix you want to take the QR
// Output:
//   Q - the Q matrix of QR
//   R - (input is NULL) ? nothing : the R matrix of QR

// NOTE: Currently assumes R and Q are not transposed
int matrix_tt_truncated_qr(matrix_tt* Q, matrix_tt* R, int r)
{
    int Qm = Q->m; int Qn = Q->n; double* QX = Q->X;
    int Qtranspose = Q->transpose; long Qoffset = Q->offset; int Qlda = Q->lda;

    lapack_int info;

    if (Qtranspose == 0){
        if (r > Qn)
        {
            printf("ERROR: (matrix_tt_truncated_qr) r = %d is too large for Q->n = %d \n", r, Qn);
            return 1;
        }
        // The QR step
        double* tau = (double*) malloc(sizeof(double)*min(Qm,Qn));
        info = LAPACKE_dgeqrf(LAPACK_COL_MAJOR, Qm, Qn, QX + Qoffset, Qlda, tau);

        if (R != NULL){
            int Rm = R->m; int Rn = R->n; double* RX = R->X;
            int Rtranspose = R->transpose; long Roffset = R->offset; int Rlda = R->lda;

            if ((Rm != r) || (Rn != r)){
                printf("ERROR: (matrix_tt_truncated_qr) R->m = %d or R->n = %d is not equal to r = %d\n", Rm, Rn, r);
                return 1;
            }
            if (Rtranspose != 0){
                printf("ERROR: (matrix_tt_truncated_qr) Only defined for R->transpose = 0\n");
                return 1;
            }

            for (int jj = 0; jj < r; ++jj){
                int r_col_length = (Qm < jj+1) ? (Qm) : jj+1;
                memcpy(RX + Roffset + jj*Rlda, QX + Qoffset + jj*Qlda, r_col_length*sizeof(double));
                memset(RX + Roffset + jj*Rlda + r_col_length, 0, (r-r_col_length)*sizeof(double));
            }
        }

        if (r > Qm){
            LAPACKE_dorgqr(LAPACK_COL_MAJOR, Qm, Qm, Qm, QX + Qoffset, Qlda, tau);
            matrix_tt* Q_sub = submatrix(Q, 0, Qm, Qm, r);
            matrix_tt_fill_zeros(Q_sub);
            free(Q_sub);
        }
        else{
            LAPACKE_dorgqr(LAPACK_COL_MAJOR, Qm, r, r, QX + Qoffset, Qlda, tau);
        }

        Q->n = r;

        free(tau);       tau = NULL;
    }
    else{
        if (r > Qm)
        {
            printf("ERROR: (matrix_tt_truncated_qr) r = %d is too large for Q->n = %d \n", r, Qn);
            return 1;
        }
        // The LQ step
        printf("Performing LQ\n");
        double* tau = (double*) malloc(sizeof(double)*min(Qm,Qn));
        info = LAPACKE_dgelqf(LAPACK_COL_MAJOR, Qm, Qn, QX + Qoffset, Qlda, tau);
        printf("\nRight after LQ, Q = \n");
        matrix_tt_print(Q, 1);

        if (R != NULL){
            int Rm = R->m; int Rn = R->n; double* RX = R->X;
            int Rtranspose = R->transpose; long Roffset = R->offset; int Rlda = R->lda;

            if ((Rm != r) || (Rn != r)){
                printf("ERROR: (matrix_tt_truncated_qr) R->m = %d or R->n = %d is not equal to r = %d\n", Rm, Rn, r);
                return 1;
            }
            if (Rtranspose == 0){
                printf("ERROR: (matrix_tt_truncated_qr) Only defined for R->transpose = Q->transpose\n");
                return 1;
            }

            int ncols = (r > Qm) ? Qm : r;
            for (int jj = 0; jj < r; ++jj){
                if (jj < Qm){
                    memcpy(RX + Roffset + jj*Rlda + jj, QX + Qoffset + jj*Qlda + jj, (Qm - jj)*sizeof(double));
                    memset(RX + Roffset + jj*Rlda, 0, jj*sizeof(double));
                }
            }

        }

        if (r > Qn){
            printf("Doing first dorglq\n");
            LAPACKE_dorglq(LAPACK_COL_MAJOR, Qn, Qn, Qn, QX + Qoffset, Qlda, tau);
            matrix_tt* Q_sub = submatrix(Q, Qn, r, 0, Qn);
            matrix_tt_fill_zeros(Q_sub);
            free(Q_sub);
        }
        else{
            printf("Doing second dorglq\n");
            LAPACKE_dorglq(LAPACK_COL_MAJOR, Qm, Qn, Qm, QX + Qoffset, Qlda, tau);
        }

        Q->m = r;

        free(tau);       tau = NULL;
    }

    return 0;
}/**/

void matrix_tt_group_reduce(MPI_Comm comm, int rank, matrix_tt* A, matrix_tt* buf, int head, int* group_ranks, int nranks)
{
    int in_the_group = 0;
    for (int ii = 0; ii < nranks; ++ii){
        if (rank == group_ranks[ii]){
            in_the_group = 1;
        }
    }

    if (in_the_group){
        int buf_assigned = 0;
        if (!buf){
            buf_assigned = 1;
            buf = matrix_tt_init(A->m, A->n);
        }

        if ((buf->m < A->m) || (buf->n < A->n)){
            printf("buf (%d x %d) must be larger than A (%d x %d)\n", buf->m, buf->n, A->m, A->n);
        }

        int send = -1;
        int recv = -1;
        while (nranks > 1){
            int half_nranks = (nranks + 1) / 2;
            for (int ii = 0; ii < half_nranks; ++ii){
                if (ii + half_nranks >= nranks){
                    recv = group_ranks[ii];
                    send = -1;
                }
                else if (group_ranks[ii + half_nranks] == head){
                    recv = head;
                    send = group_ranks[ii];
                }
                else{
                    recv = group_ranks[ii];
                    send = group_ranks[ii + half_nranks];
                }

                if (rank == send){
                    matrix_tt_send(comm, A, recv);
                }
                else if ((rank == recv) && (send != -1)){
                    matrix_tt_recv(comm, buf, send);
                    for (int jj = 0; jj < A->n; ++jj){
//                        MPI_Recv(buf->X + buf->offset, A->m, MPI_DOUBLE, send, 0, comm, MPI_STATUS_IGNORE);
                        long A_offset = A->offset + jj * (A->lda);
                        long buf_offset = buf->offset +  jj * (buf->lda);
                        for (int kk = 0; kk < A->m; ++kk){
                            A->X[A_offset + kk] = A->X[A_offset + kk] + buf->X[buf_offset + kk];
                        }
                    }
                }
                group_ranks[ii] = recv;
            }
            nranks = half_nranks;
        }

        if (buf_assigned){
            matrix_tt_free(buf);
        }
    }

}

// buf just has to be size A->lda x 1 or more
void matrix_tt_reduce(MPI_Comm comm, int rank, matrix_tt* A, matrix_tt* buf, int head)
{
    if (buf == NULL){
        matrix_tt* buf = matrix_tt_init(A->m, 1);
        matrix_tt_reduce(comm, rank, A, buf, head);
        matrix_tt_free(buf);
    }
    else{
        int size;
        MPI_Comm_size(comm, &size);

        if ((head < 0) || (head >= size)){
            printf("matrix_tt_reduce: head = %d is not a valid rank for size = %d\n", head, size);
        }

        if (buf->lda < A->m){
            printf("buf->lda = %d must be larger than A->m = %d\n", buf->lda, A->m);
        }

        int* activated_ranks = (int*) calloc(size, sizeof(int));
        for (int ii = 0; ii < size; ++ii){
            activated_ranks[ii] = ii;
        }
        int send = -1;
        int recv = -1;
        while (size > 1){
            int half_size = (size + 1) / 2;
            for (int ii = 0; ii < half_size; ++ii){
                if (ii + half_size >= size){
                    recv = activated_ranks[ii];
                    send = -1;
                }
                else if (activated_ranks[ii + half_size] == head){
                    recv = head;
                    send = activated_ranks[ii];
                }
                else{
                    recv = activated_ranks[ii];
                    send = activated_ranks[ii + half_size];
                }

                if (rank == send){
                    for (int jj = 0; jj < A->n; ++jj){
                        MPI_Send(A->X + A->offset + A->lda * jj, A->m, MPI_DOUBLE, recv, 0, comm);
                    }
                }
                else if ((rank == recv) && (send != -1)){
                    for (int jj = 0; jj < A->n; ++jj){
                        MPI_Recv(buf->X + buf->offset, A->m, MPI_DOUBLE, send, 0, comm, MPI_STATUS_IGNORE);
                        long A_offset = A->offset + jj * (A->lda);
                        for (int kk = 0; kk < A->m; ++kk){
                            A->X[A_offset + kk] = A->X[A_offset + kk] + buf->X[buf->offset + kk];
                        }
                    }
                }
                activated_ranks[ii] = recv;
            }
            size = half_size;
        }
        free(activated_ranks);
    }
}

void matrix_tt_allreduce(MPI_Comm comm, matrix_tt* A)
{
    for (int jj = 0; jj < A->n; ++jj){
        MPI_Allreduce(MPI_IN_PLACE, A->X + A->offset + jj*(A->lda), A->m, MPI_DOUBLE, MPI_SUM, comm);
    }
}

void matrix_tt_broadcast(MPI_Comm comm, matrix_tt* buf)
{
    int count = buf->m * buf->n;
    MPI_Bcast(buf->X, count, MPI_DOUBLE, HEAD, comm);
}

void matrix_tt_send(MPI_Comm comm, matrix_tt* buf, int dest){
    MPI_Datatype buf_type;
    MPI_Type_vector(buf->n, buf->m, buf->lda, MPI_DOUBLE, &buf_type);
    MPI_Type_commit(&buf_type);
    MPI_Send(buf->X + buf->offset, 1, buf_type, dest, 0, comm);
    MPI_Type_free(&buf_type);
}

void matrix_tt_recv(MPI_Comm comm, matrix_tt* buf, int source){
    MPI_Datatype buf_type;
    MPI_Type_vector(buf->n, buf->m, buf->lda, MPI_DOUBLE, &buf_type);
    MPI_Type_commit(&buf_type);
    MPI_Recv(buf->X + buf->offset, 1, buf_type, source, 0, comm, MPI_STATUS_IGNORE);
    MPI_Type_free(&buf_type);
}

// Column khatri-rao product C = A x B
void khatri_rao(const matrix_tt* restrict A, const matrix_tt* restrict B, matrix_tt* restrict C){
    int n = A->n;
    int m_A = A->m;

    int n_B = B->n;
    int m_B = B->m;

    int n_C = C->n;
    int m_C = C->m;

    if ((n != n_B) || (n != n_C) || (m_A*m_B != m_C)){
        printf("khatri_rao: the sizes dont work!\n");
        printf("A is %d x %d\n", m_A, n);
        printf("B is %d x %d\n", m_B, n_B);
        printf("C is %d x %d\n", m_C, n_C);
    }

    for (int kk = 0; kk < n; ++kk){
        for (int ii = 0; ii < m_B; ++ii){
            double Bii = B->X[kk*m_B + ii];
            for (int jj = 0; jj < m_A; ++jj){
                C->X[kk*m_C + ii*m_B + jj] = Bii * A->X[kk*m_A + jj];
            }
        }
    }
}

// Recursively KR multiply the list of matrices A
void list_khatri_rao(int d_kr, matrix_tt** A, matrix_tt* Omega){
    if (d_kr == 1){
        for (int ii = 0; ii < A[0]->X_size; ++ii){
            Omega->X[ii] = A[0]->X[ii];
        }
        return;
    }

    if (d_kr == 2){
        khatri_rao(A[0], A[1], Omega);
    }
    else{
        int m_B = 1;
        int n_B = Omega->n;
        for (int ii = 0; ii < d_kr-1; ++ii){
            m_B = m_B * A[ii]->m;
        }

        matrix_tt* B = matrix_tt_init(m_B, n_B);
        list_khatri_rao(d_kr - 1, A, B);
        khatri_rao(B, A[d_kr - 1], Omega);
        matrix_tt_free(B); B = NULL;
    }

}

void matrix_tt_dlarnv(matrix_tt* A){
    int r1 = rand()%4096, r2 = rand()%4096, r3 = rand()%4096, r4 = rand()%4096;
    int iseed[4] = {r1, r2, r3, r4+(r4%2 == 0?1:0)};
    LAPACKE_dlarnv(3, iseed, (A->n) * (A->m), A->X);
}

void matrix_tt_fill_zeros(matrix_tt* A)
{
    for (int jj = 0; jj < A->n; ++jj){
        memset(A->X + A->offset + jj*A->lda, 0, A->m * sizeof(double));
    }
}

// Takes a matrix in row major and transforms it to column major
void row_to_col_major(const matrix_tt* mat_row, matrix_tt* mat_col)
{
    int m = mat_col->m;
    int n = mat_col->n;
    if ((m != mat_row->n) || (n != mat_row->m)){
        printf("The dimensions don't work out! mat_row should have the dimensions of mat_col transposed\n");
        printf("mat_row->m = %d, mat_row->n = %d\n", mat_row->m, mat_row->n);
        printf("mat_col->m = %d, mat_col->n = %d\n", mat_col->m, mat_col->n);
    }

    for (int ii = 0; ii < m; ++ii){
        for (int jj = 0; jj < n; ++jj){
            mat_col->X[jj * (mat_col->lda) + ii + mat_col->offset] = mat_row->X[ii * (mat_row->lda) + jj + mat_row->offset];
        }
    }
}


/* wavelet/gsl_wavelet.h
 *
 * Copyright (C) 2004 Ivo Alxneit
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_WAVELET_H__
#define __GSL_WAVELET_H__
#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS          /* empty */
# define __END_DECLS            /* empty */
#endif

__BEGIN_DECLS

#ifndef GSL_DISABLE_DEPRECATED
typedef enum {
  forward = 1, backward = -1,
  gsl_wavelet_forward = 1, gsl_wavelet_backward = -1
}
gsl_wavelet_direction;
#else
typedef enum {
  gsl_wavelet_forward = 1, gsl_wavelet_backward = -1
}
gsl_wavelet_direction;
#endif

typedef struct
{
  const char *name;
  int (*init) (const double **h1, const double **g1,
               const double **h2, const double **g2, size_t * nc,
               size_t * offset, size_t member);
}
gsl_wavelet_type;

typedef struct
{
  const gsl_wavelet_type *type;
  const double *h1;
  const double *g1;
  const double *h2;
  const double *g2;
  size_t nc;
  size_t offset;
}
gsl_wavelet;

typedef struct
{
  double *scratch;
  size_t n;
}
gsl_wavelet_workspace;

GSL_VAR const gsl_wavelet_type *gsl_wavelet_daubechies;
GSL_VAR const gsl_wavelet_type *gsl_wavelet_daubechies_centered;
GSL_VAR const gsl_wavelet_type *gsl_wavelet_haar;
GSL_VAR const gsl_wavelet_type *gsl_wavelet_haar_centered;
GSL_VAR const gsl_wavelet_type *gsl_wavelet_bspline;
GSL_VAR const gsl_wavelet_type *gsl_wavelet_bspline_centered;

gsl_wavelet *gsl_wavelet_alloc (const gsl_wavelet_type * T, size_t k);
void gsl_wavelet_free (gsl_wavelet * w);
const char *gsl_wavelet_name (const gsl_wavelet * w);

gsl_wavelet_workspace *gsl_wavelet_workspace_alloc (size_t n);
void gsl_wavelet_workspace_free (gsl_wavelet_workspace * work);

int gsl_wavelet_transform (const gsl_wavelet * w,
                           double *data, size_t stride, size_t n,
                           gsl_wavelet_direction dir,
                           gsl_wavelet_workspace * work);

int gsl_wavelet_transform_forward (const gsl_wavelet * w,
                                   double *data, size_t stride, size_t n,
                                   gsl_wavelet_workspace * work);

int gsl_wavelet_transform_inverse (const gsl_wavelet * w,
                                    double *data, size_t stride, size_t n,
                                    gsl_wavelet_workspace * work);

__END_DECLS

#endif /* __GSL_WAVELET_H__ */


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include <memory>
#include <vector>
#include <gsl/span>
#include <map>

#include <Eigen/Geometry>

namespace mage
{
    struct BundlerParameters
    {
        bool  ArePointsFixed{ false };        // True if map points should not be optimized
    };

    class BundlerLib
    {
    public:
        BundlerLib(const BundlerParameters& bundlerParameters);
        ~BundlerLib();

        void AllocateCameras(size_t count);

        void SetCameraPose(size_t idx,
            Eigen::Map<const Eigen::Vector3f> position,
            Eigen::Map<const Eigen::Matrix3f> orientation,
            Eigen::Map<const Eigen::Vector4f> intrinsics, bool isFixed);

        void FixCameraPose(size_t idx, bool value);

        void AllocateMapPoints(size_t count);
        void SetMapPoint(size_t idx, Eigen::Map<const Eigen::Vector3f> point);

        void AllocateObservations(size_t count);
        void SetObservation(size_t idx, Eigen::Map<const Eigen::Vector2f> position, size_t cameraIndex, size_t mapPointIndex, float informationMatrixScalar);

        void AllocateFixedDistanceConstraints(size_t count);
        void SetFixedDistanceConstraint(size_t idx, size_t cameraIndex1, size_t cameraIndex2, float distance = 1.0f, float weight = 1.0f);

        void AllocateRelativeRotationConstraints(size_t count);
        void SetRelativeRotationConstraint(size_t idx, size_t cameraIndex1, size_t cameraIndex2, const Eigen::Quaternionf& deltaRotation, float weight = 1.0f);

        void AllocateRelativeTransformConstraints(size_t count);
        void SetRelativeTransformConstraint(size_t idx, size_t cameraIndex1, size_t cameraIndex2, Eigen::Map<const Eigen::Vector3f> deltaPosition, const Eigen::Quaternionf& deltaRotation, float weight);

        void SetCurrentLambda(float userLambda);
        float GetCurrentLambda() const;

        // Runs an iteration of the solver for each provided Huber width.
        // Return the average square error.
        float StepBundleAdjustment(gsl::span<const float> huberWidthPerIteration, float maxErrorSquare, std::vector<unsigned int>& outliers);

        void GetPose(size_t idx, Eigen::Map<Eigen::Vector3f> position, Eigen::Map<Eigen::Matrix3f> orientation) const;
        void GetPoint(size_t idx, Eigen::Map<Eigen::Vector3f> position) const;

    private:
        struct Impl;
        const std::unique_ptr<Impl> m_impl;

        // Description of the problem to optimize from the calling code.
        BundlerParameters m_bundlerParameters;
    };
}


/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2013, Georgia Tech Research Corporation
 * Copyright (c) 2015, Rice University
 * Copyright (c) 2018-2019, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@gatech.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   * Neither the name of the Rice University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <nlopt.h>
#include "amino.h"
#include "amino/diffeq.h"

/* static double */
/* s_nlobj_jpinv(unsigned n, const double *q, double *dq, void *vcx) */
/* { */
/*     struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx; */
/*     void *ptrtop = aa_mem_region_ptr(cx->reg); */

/*     struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1); */
/*     aa_rx_fk_sub(cx->fk, cx->ssg, &vq); */
/*     double *E_act = aa_rx_fk_ref(cx->fk, cx->frame); */

/*     if( dq ) { */
/*         struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1); */
/*         s_ksol_jpinv(cx,q, &v_dq); */
/*         aa_dvec_scal(-1,&v_dq); */
/*     } */

/*     double x = s_serr( E_act, cx->TF_ref->data ); */

/*     aa_mem_region_pop(cx->reg, ptrtop); */
/*     return x; */

/* } */

static double s_nlobj_dq_fd_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);

    double S_act[8], S_ref[8], S_err[8], S_ln[8];
    aa_tf_qutr2duqu(E_act, S_act);
    aa_tf_qutr2duqu(cx->TF_ref->data, S_ref);
    aa_tf_duqu_cmul(S_act,S_ref,S_err);
    aa_tf_duqu_minimize(S_err);
    aa_tf_duqu_ln(S_err, S_ln);
    double result = aa_la_dot(8,S_ln,S_ln);

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

AA_API double
aa_rx_ik_opt_err_dqln_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_dq_fd_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_dq_fd_helper, vcx,
                       &vq, eps, &vdq );
    }

    return x;
}

static void
mv_block_helper (const double *A, const double *x, double *y)
{
    cblas_dgemv(CblasColMajor, CblasTrans, 8, 4,
                1, A, 8, x, 1,
                0, y, 1);
    cblas_dgemv(CblasColMajor, CblasTrans, 4, 4,
                1, A, 8, x+4, 1,
                0, y+4, 1);
}

static void
duqu_rmul_helper( const double *Sr, const double *x, double *y )
{
    double M[8*8];
    aa_tf_duqu_matrix_r(Sr,M,8);
    mv_block_helper(M, x, y);
}

AA_API double
aa_rx_ik_opt_err_dqln( void *vcx, double *q, double *dq ) {
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);

    double S_act[8], S_ref[8], S_err[8], S_ln[8];
    aa_tf_qutr2duqu(E_act, S_act);
    aa_tf_qutr2duqu(cx->TF_ref->data, S_ref);
    aa_tf_duqu_cmul(S_act,S_ref,S_err);
    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion
    int needs_min = (S_err[AA_TF_DUQU_REAL_W] < 0 );
    if( needs_min ) {
        for( size_t i = 0; i < 8; i ++ ) S_err[i] *= -1;
    } else {
    }
    aa_tf_duqu_ln(S_err, S_ln);
    double result = aa_la_dot(8,S_ln,S_ln);

    if( dq ) {
        if( needs_min ) {
            for( size_t i = 0; i < 8; i ++ ) S_ln[i] *= -1;
        }
        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        /*
         * g_sumsq * J_ln*[S_ref]_r*J_conj*J_S
         * -> J_ln^T g_sumsq * [S_ref]_r*J_conj*J_S
         * -> [S_ref]_r^T (J_ln^T g_sumsq) *J_conj*J_S
         * -> J_conj^T*[S_ref]_r^T (J_ln^T g_sumsq) * J_S
         * -> J_S^T J_conj^T * [S_ref]_r^T * J_ln^T g_sumsq
         */

        double a[8], b[8], dJ[8*8];
        struct aa_dmat vJ_8x8 = AA_DMAT_INIT(8,8,dJ,8);
        struct aa_dmat *J_8x8 = &vJ_8x8;

        {
            struct aa_dmat *J_ln = J_8x8;
            aa_tf_duqu_ln_jac(S_err,J_ln);
            /*
             *  struct aa_dvec v_S_ln = AA_DVEC_INIT(8,S_ln,1);
             * //aa_dmat_gemv( CblasTrans, 2, J_8x8, &v_S_ln, 0, &va );
             *
             * Avoid multiplying by the zero block:
             *
             * J_ln = [J_r   0]     J_ln^T = [J_r^T J_d^T]
             *        [J_d J_r]              [  0   J_r^T]
             *
             * J_ln^T * del(S_ln^T*S_ln)^T = 2*J_ln^T S_ln
             *
             * 2*J_ln^T S_ln = 2*[J_r^T  J_d^T] (S_ln_r)
             *                   [   0   J_r^T] (S_ln_d)
             *
             */
            mv_block_helper(dJ, S_ln, a);
        }

        /*
         * dS/dphi = [S]_R V J
         * g [S]_R V J
         * -> [S]_R^T g V J
         * -> V^T [S]_R^T g J
         * -> J^T V [S]_R^T g
         */

        duqu_rmul_helper( S_ref, a, b );
        aa_tf_duqu_conj1(b);

        duqu_rmul_helper( S_act, b, a );  /* TODO: Pure result, some
                                           * extra multiplies here. */
        // a is now a pure dual quaternion
        {
            double c[6];
            struct aa_dvec vc = AA_DVEC_INIT(6,c,1);
            // Using Twist Jacobian
            aa_tf_duqu2pure(a,c);
            struct aa_dmat *Jtw = aa_rx_sg_sub_jac_twist_get(cx->ssg, cx->reg, cx->fk);
            aa_dmat_gemv( CblasTrans, 1, Jtw, &vc, 0, &v_dq );

            // Using Velocity Jacobian
            /* aa_tf_cross_a(a+AA_TF_DUQU_DUAL_XYZ,E_act+AA_TF_QUTR_V,a); */
            /* aa_tf_duqu2pure(a,c); */
            /* struct aa_dmat *J_vel = aa_rx_sg_sub_get_jacobian(cx->ssg,cx->reg,cx->TF); */
            /* aa_dmat_gemv( CblasTrans, 1, J_vel, &vc, 0, &v_dq ); */

        }

        /* { */
        /*     struct aa_dvec *v_dq_fd = aa_dvec_alloc(cx->reg,n); */
        /*     double eps = 1e-6; */
        /*     aa_de_grad_fd( s_nlobj_dq_fd_helper, vcx, */
        /*                    &vq, eps, v_dq_fd ); */

        /*     printf("--\n"); */
        /*     printf("needs min: %d\n", needs_min); */
        /*     printf("ad: "); */
        /*     aa_dump_vec(stdout,v_dq_fd->data,n); */
        /*     printf("an: "); */
        /*     aa_dump_vec(stdout,dq,n); */

        /*     assert( aa_dvec_ssd(v_dq_fd, &v_dq) < 1e-3 ); */
        /* } */
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

static double s_nlobj_qv_fd_trans_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = aa_tf_vdot(v_err,v_err);

    return result;
}

AA_API double
aa_rx_ik_opt_err_trans_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_qv_fd_trans_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_qv_fd_trans_helper, vcx,
                       &vq, eps, &vdq );
    }
    /* fprintf(stdout, "fd error: %2.2f\n", x);  */
    return x;
}

static double s_nlobj_qv_fd_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;
    double *q_act = E_act + AA_TF_QUTR_Q;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;
    double *q_ref = E_ref + AA_TF_QUTR_Q;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;
    double *q_err = E_err + AA_TF_QUTR_Q;

    aa_tf_qcmul(q_act,q_ref,q_err);

    double q_ln[4];
    aa_tf_qminimize(q_err);
    aa_tf_duqu_ln(q_err, q_ln);

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = ( aa_tf_qdot(q_ln,q_ln)
                      +
                      aa_tf_vdot(v_err,v_err) );

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

AA_API double
aa_rx_ik_opt_err_qlnpv_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_qv_fd_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_qv_fd_helper, vcx,
                       &vq, eps, &vdq );
    }

    return x;
}

AA_API double
aa_rx_ik_opt_err_trans( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );

    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;

    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = aa_tf_vdot(v_err,v_err);

    if( dq ) {

        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        struct aa_dmat *Jvel = aa_rx_sg_sub_jac_vel_get(cx->ssg, cx->reg, cx->fk);
        struct aa_dmat Jr, Jv;
        aa_dmat_view_block(&Jv, Jvel, AA_TF_DX_V, 0, 3, Jvel->cols);
        aa_dmat_view_block(&Jr, Jvel, AA_TF_DX_W, 0, 3, Jvel->cols);

        // Translational Part
        struct aa_dvec v_v_err = AA_DVEC_INIT(3,v_err,1);
        aa_dmat_gemv(CblasTrans, 2, &Jv, &v_v_err, 0, &v_dq);
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    /* printf("err: %f\n", result); */
    return result;
}

static void
q_rmul_helper( const double *q, const struct aa_dvec *x, struct aa_dvec *y )
{
    double dM[4*4];
    struct aa_dmat M = AA_DMAT_INIT(4,4,dM,4);
    aa_tf_qmat_r(q,&M);
    aa_dmat_gemv(CblasTrans, 1, &M, x, 0, y);
}

AA_API double
aa_rx_ik_opt_err_qlnpv( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );

    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;
    double *q_act = E_act + AA_TF_QUTR_Q;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;
    double *q_ref = E_ref + AA_TF_QUTR_Q;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;
    double *q_err = E_err + AA_TF_QUTR_Q;

    aa_tf_qcmul(q_act,q_ref,q_err);

    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion
    int needs_min = (E_err[AA_TF_QUAT_W] < 0 );
    if( needs_min ) {
        for( size_t i = 0; i < 4; i ++ ) E_err[i] *= -1;
    }

    double q_ln[4];
    aa_tf_qln(E_err, q_ln);

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = ( aa_tf_qdot(q_ln,q_ln) +
                      + aa_tf_vdot(v_err,v_err) );

    if( dq ) {

        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        struct aa_dmat *Jvel = aa_rx_sg_sub_jac_vel_get(cx->ssg, cx->reg, cx->fk);
        struct aa_dmat Jr, Jv;
        aa_dmat_view_block(&Jv, Jvel, AA_TF_DX_V, 0, 3, Jvel->cols);
        aa_dmat_view_block(&Jr, Jvel, AA_TF_DX_W, 0, 3, Jvel->cols);

        // Translational Part
        struct aa_dvec v_v_err = AA_DVEC_INIT(3,v_err,1);
        aa_dmat_gemv(CblasTrans, 2, &Jv, &v_v_err, 0, &v_dq);

        // Rotational Part
        if( needs_min ) {
            for( size_t i = 0; i < 4; i ++ ) q_ln[i] *= -1;
        }

        double a[4], b[4], dJ[4*4];
        struct aa_dvec va = AA_DVEC_INIT(4,a,1);
        struct aa_dvec vb = AA_DVEC_INIT(4,b,1);
        struct aa_dvec vln = AA_DVEC_INIT(4,q_ln,1);
        struct aa_dmat vJ_4x4 = AA_DMAT_INIT(4,4,dJ,4);
        struct aa_dmat *J_4x4 = &vJ_4x4;

        {
            struct aa_dmat *J_ln = J_4x4;
            aa_tf_qln_jac(q_err,J_ln);
            aa_dmat_gemv(CblasTrans, 1, J_ln, &vln, 0, &va);
        }

        q_rmul_helper( q_ref, &va, &vb );
        aa_tf_qconj1(b);

        q_rmul_helper( q_act, &vb, &va ); /* TODO: Pure result, some
                                           * extra multiplies here. */

        // a is now a pure dual quaternion */
        {
            struct aa_dvec va3 = AA_DVEC_INIT(3,a,1);
            aa_dmat_gemv( CblasTrans, 1, &Jr, &va3, 1, &v_dq );
        }

        /* { */
        /*     struct aa_dvec *v_dq_fd = aa_dvec_alloc(cx->reg,n); */
        /*     double eps = 1e-6; */
        /*     aa_de_grad_fd( s_nlobj_qv_fd_helper, vcx, */
        /*                    &vq, eps, v_dq_fd ); */

        /*     printf("--\n"); */
        /*     printf("needs min: %d\n", needs_min); */
        /*     printf("ad: "); */
        /*     aa_dump_vec(stdout,v_dq_fd->data,n); */
        /*     printf("an: "); */
        /*     aa_dump_vec(stdout,dq,n); */

        /*     assert( aa_dvec_ssd(v_dq_fd, &v_dq) < 1e-3 ); */
        /* } */
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    //printf("err: %f\n", result);
    return result;
}

// TODO: weighted error

AA_API double
aa_rx_ik_opt_err_jcenter( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    struct aa_mem_region *reg = cx->reg;
    void *ptrtop = aa_mem_region_ptr(reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec *q_center = aa_dvec_alloc(reg,n);
    aa_rx_sg_sub_center_configv(cx->ssg,q_center);

    double result = 0;

    if( dq ) { // error and gradient
        for( size_t i = 0; i < n; i ++ ) {
            double d = q[i] - AA_DVEC_REF(q_center,i);
            result += (d*d);
            dq[i] = d;
        }
        //printf("--\n");
        //printf("q:  "); aa_dump_vec(stdout,q,n);
        //printf("qc: "); aa_dump_vec(stdout,q_center->data,n);
        //printf("dq: "); aa_dump_vec(stdout,dq,n);
    } else { // error only
        for( size_t i = 0; i < n; i ++ ) {
            double d = q[i] - AA_DVEC_REF(q_center,i);
            result += (d*d);
        }
    }

    aa_mem_region_pop(cx->reg, ptrtop);

    return result / 2;
}

struct err_cx {
    void *cx;
    aa_rx_ik_opt_fun *fun;
};

static double
s_err_cx_dispatch(unsigned n, const double *q, double *dq, void *vcx)
{
    (void)n;
    struct err_cx *cx = (struct err_cx *)vcx;
    return cx->fun(cx->cx, q, dq);
}

static int
s_ik_nlopt( struct kin_solve_cx *cx,
            struct aa_dvec *q )
{
    struct aa_mem_region *reg = cx->reg;
    void *ptrtop = aa_mem_region_ptr(reg);

    const struct aa_rx_sg_sub *ssg = cx->ssg;
    const struct aa_rx_sg *sg = cx->ssg->scenegraph;

    if( 1 != cx->q_sub->inc || 1 != cx->q_all->inc ) {
        return AA_RX_INVALID_PARAMETER;
    }

    size_t n_sub = cx->q_sub->len;
    nlopt_algorithm alg = NLOPT_LD_SLSQP;
    nlopt_opt opt = nlopt_create(alg, (unsigned)n_sub); /* algorithm and dimensionality */

    struct err_cx ocx, eqctcx;
    ocx.cx = cx;
    ocx.fun = cx->ik_cx->opts->obj_fun;
    nlopt_set_min_objective(opt, s_err_cx_dispatch, &ocx);

    if( cx->ik_cx->opts->eqct_fun ) {
        eqctcx.cx = cx;
        eqctcx.fun = cx->ik_cx->opts->eqct_fun;
        nlopt_add_equality_constraint(opt, s_err_cx_dispatch, &eqctcx,
                                      cx->ik_cx->opts->eqct_tol);
    }

    //nlopt_set_xtol_rel(opt, 1e-4); // TODO: make a parameter
    if( cx->opts->tol_obj_abs >= 0 )
        nlopt_set_ftol_abs(opt, cx->opts->tol_obj_abs );
    if( cx->opts->tol_obj_rel >= 0 )
        nlopt_set_ftol_rel(opt, cx->opts->tol_obj_rel );
    if( cx->opts->tol_dq >= 0 )
        nlopt_set_xtol_rel(opt, cx->opts->tol_dq );

    double *lb = AA_MEM_REGION_NEW_N(reg,double,n_sub);
    double *ub = AA_MEM_REGION_NEW_N(reg,double,n_sub);
    { // bounds
        for( size_t i = 0; i < n_sub; i ++ ) {
            aa_rx_config_id id = aa_rx_sg_sub_config(ssg,i);
            if( aa_rx_sg_get_limit_pos(sg, id, lb+i, ub+i) )
            {
                lb[i] = -DBL_MAX;
                ub[i] = DBL_MAX;
            }
        }
    }

    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_upper_bounds(opt, ub);
    double minf;
    nlopt_result ores = nlopt_optimize(opt, cx->q_sub->data, &minf);
    if( cx->opts->debug ) {
        /* fprintf("AMINO IK NLOPT RESULT: %s (%d)", */
        /*         nlopt_result_to_string(ores), ores); */

        fprintf(stderr, "AMINO IK NLOPT RESULT: %d\n", ores);
    }
    aa_dvec_copy( cx->q_sub, q );

    aa_rx_fk_sub(cx->fk, cx->ssg, q);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    int result = s_check(cx->ik_cx, cx->TF_ref, E_act );
    nlopt_destroy(opt);
    aa_mem_region_pop(reg,ptrtop);

    return result;

}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for the conversion between time scales.
 *
 */

#ifndef _TIMECONVERSION_H
#define _TIMECONVERSION_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>

#include "constants.h"

/*************************/
/****** Prototypes ******/

/* Function computing the gmst angle from the gps time */
double gmst_angle_from_gpstime(const double gpstime); /* gpstime in seconds */

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _TIMECONVERSION_H */


#ifndef OPENMC_TALLIES_FILTER_ENERGY_H
#define OPENMC_TALLIES_FILTER_ENERGY_H

#include <gsl/gsl>

#include "openmc/tallies/filter.h"
#include "openmc/vector.h"

namespace openmc {

//==============================================================================
//! Bins the incident neutron energy.
//==============================================================================

class EnergyFilter : public Filter {
public:
  //----------------------------------------------------------------------------
  // Constructors, destructors

  ~EnergyFilter() = default;

  //----------------------------------------------------------------------------
  // Methods

  std::string type() const override { return "energy"; }

  void from_xml(pugi::xml_node node) override;

  void get_all_bins(const Particle& p, TallyEstimator estimator,
    FilterMatch& match) const override;

  void to_statepoint(hid_t filter_group) const override;

  std::string text_label(int bin) const override;

  //----------------------------------------------------------------------------
  // Accessors

  const vector<double>& bins() const { return bins_; }
  void set_bins(gsl::span<const double> bins);

  bool matches_transport_groups() const { return matches_transport_groups_; }

protected:
  //----------------------------------------------------------------------------
  // Data members

  vector<double> bins_;

  //! True if transport group number can be used directly to get bin number
  bool matches_transport_groups_ {false};
};

//==============================================================================
//! Bins the outgoing neutron energy.
//!
//! Only scattering events use the get_all_bins functionality.  Nu-fission
//! tallies manually iterate over the filter bins.
//==============================================================================

class EnergyoutFilter : public EnergyFilter {
public:
  //----------------------------------------------------------------------------
  // Methods

  std::string type() const override { return "energyout"; }

  void get_all_bins(const Particle& p, TallyEstimator estimator,
    FilterMatch& match) const override;

  std::string text_label(int bin) const override;
};

} // namespace openmc
#endif // OPENMC_TALLIES_FILTER_ENERGY_H