Enhanced SIM for E2AP v1 for TS UC

Issue-ID: SIM-18

Signed-off-by: Ron Shacham <rshacham@research.att.com>
Change-Id: I004901af3f1526f6c85400a5f6d564d5ea051501
diff --git a/e2sim/e2apv1sim/test/Pendulum/Pendulum_asn_codec.c b/e2sim/e2apv1sim/test/Pendulum/Pendulum_asn_codec.c
new file mode 100644
index 0000000..9b44adf
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/Pendulum_asn_codec.c
@@ -0,0 +1,180 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+#include "Pendulum_asn_codec.h"
+#include "per_decoder.h"
+#include "per_encoder.h"
+
+#include "OCTET_STRING.h"
+
+static int ASN_DEBUG = 0;
+
+int pendulum_asn_encode(Pendulum_t *pend, uint8_t **buffer, uint32_t *len)
+{
+  ssize_t encoded;
+  assert(pend != NULL);
+  assert(buffer != NULL);
+
+  encoded = aper_encode_to_new_buffer(&asn_DEF_Pendulum, 0, pend, (void **)buffer);
+  if(encoded < 0){
+    perror("Failed to aper encode\n");
+    exit(1);
+  }
+
+  *len = encoded;
+  //ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_Pendulum, pend);
+  return encoded;
+}
+
+int pendulum_asn_decode(Pendulum_t *pend, const uint8_t *const buffer, const int len)
+{
+  asn_dec_rval_t dec_ret;
+
+  // THIS IS IMPORTANT, otherwise: Segmentation fault
+  memset(pend, 0, sizeof(*pend));
+
+  assert(buffer != NULL);
+
+  dec_ret = aper_decode(NULL, &asn_DEF_Pendulum, (void **)&pend, buffer, len, 0, 0);
+  if (dec_ret.code != RC_OK) {
+    fprintf(stderr, "ERROR: Failed to decode asn1 message\n");
+    return -1;
+  }
+
+  if(ASN_DEBUG)
+    xer_fprint(stdout, &asn_DEF_Pendulum, pend);
+
+  return 0;
+}
+
+int pendulum_create_asn_msg(uint8_t **buffer, long sequence,
+  double angle, double torque, const char* msg)
+{
+  //Create Pendulum payload struct
+  Pendulum_t *pend;
+
+  pend = calloc(1, sizeof(Pendulum_t));
+  if(!pend){
+    perror("calloc() failed");
+    exit(1);
+  }
+
+  //convert char* to PrintableString_t*
+  PrintableString_t  *payload = calloc(1, sizeof(PrintableString_t));
+  payload->buf = (uint8_t *)msg;
+  payload->size = strlen(msg);
+
+  pend->strval    = payload;
+  pend->sequence  = &sequence;
+  pend->angle     = &angle;
+  pend->torque    = &torque;
+
+  if (ASN_DEBUG)
+    xer_fprint(stdout, &asn_DEF_Pendulum, pend);
+
+  //Encode Pendulum payload struct to asn1 buffer
+  uint32_t  len;
+  if(pendulum_asn_encode(pend, buffer, &len) < 0)
+  {
+    return -1;
+  }
+  //fprintf(stderr, "len = %d\n", len);
+
+  return len;
+}
+
+long pendulum_get_sequence(const uint8_t *const buffer, const int len)
+{
+  Pendulum_t pend;
+  pendulum_asn_decode(&pend, buffer, len);
+
+  return *(pend.sequence);
+}
+
+double pendulum_get_angle(const uint8_t *const buffer, const int len)
+{
+  Pendulum_t pend;
+  pendulum_asn_decode(&pend, buffer, len);
+
+  return *(pend.angle);
+}
+
+double pendulum_get_torque(const uint8_t *const buffer, const int len)
+{
+  Pendulum_t pend;
+  pendulum_asn_decode(&pend, buffer, len);
+
+  return *(pend.torque);
+}
+
+char* pendulum_get_strval(const uint8_t *const buffer, const int len)
+{
+  Pendulum_t pend;
+  char* str;
+
+  pendulum_asn_decode(&pend, buffer, len);
+
+  str = (char*)pend.strval->buf;
+
+  return str;
+}
+
+void test_pendulum_msg(void)
+{
+  uint8_t   *buffer = NULL;
+  uint32_t  len = 0;
+  double    angle = 1.9;
+  len = pendulum_create_asn_msg(&buffer, 0, angle, 0, NULL);
+
+  double ex_angle = pendulum_get_angle(buffer, len);
+  fprintf(stderr, "Extracted angle = %f\n", ex_angle);
+}
+
+void test_pendulum_asn1(void)
+{
+  fprintf(stderr, "test_pendulum_asn1\n");
+
+  Pendulum_t *pend;
+
+  long	sequence = 0;	/* OPTIONAL */
+  double	angle = 1.5;	/* OPTIONAL */
+  //double	torque = 0.7;	/* OPTIONAL */
+
+  pend = calloc(1, sizeof(Pendulum_t));
+  if(!pend){
+    perror("calloc() failed");
+    exit(1);
+  }
+
+  pend->sequence  = &sequence;
+  pend->angle     = &angle;
+  //pend->torque    = &torque;
+
+  xer_fprint(stdout, &asn_DEF_Pendulum, pend);
+
+  //encode
+  uint8_t   *buffer = NULL;
+  uint32_t  len;
+  pendulum_asn_encode(pend, &buffer, &len);
+  fprintf(stderr, "len = %d\n", len);
+
+  //decode
+  Pendulum_t dec_pend;
+  pendulum_asn_decode(&dec_pend, buffer, len);
+}
diff --git a/e2sim/e2apv1sim/test/Pendulum/Pendulum_asn_codec.h b/e2sim/e2apv1sim/test/Pendulum/Pendulum_asn_codec.h
new file mode 100644
index 0000000..3928caf
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/Pendulum_asn_codec.h
@@ -0,0 +1,46 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+#ifndef PENDULUM_ASN_CODEC_H_
+#define PENDULUM_ASN_CODEC_H_
+
+#include "Pendulum.h"
+
+int pendulum_asn_encode(Pendulum_t *pend, uint8_t **buffer, uint32_t *len);
+
+int pendulum_asn_decode(Pendulum_t *pend, const uint8_t *const buffer, const int len);
+
+int pendulum_create_asn_msg(uint8_t **buffer, long sequence,
+  double angle, double torque, const char* msg);
+
+long pendulum_get_sequence(const uint8_t *const buffer, const int len);
+
+double pendulum_get_angle(const uint8_t *const buffer, const int len);
+
+double pendulum_get_torque(const uint8_t *const buffer, const int len);
+
+char*  pendulum_get_strval(const uint8_t *const buffer, const int len);
+
+//For testing only
+
+void test_pendulum_msg(void);
+
+void test_pendulum_asn1(void);
+
+#endif
diff --git a/e2sim/e2apv1sim/test/Pendulum/Serial/adruino_serial.c b/e2sim/e2apv1sim/test/Pendulum/Serial/adruino_serial.c
new file mode 100644
index 0000000..e67465c
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/Serial/adruino_serial.c
@@ -0,0 +1,98 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+#include "stdio.h"
+#include <string.h>
+#include <unistd.h>
+
+#include "adruino_serial.h"
+
+int start_serial_inferface(int baudrate, char *serial_port)
+{
+  int fd = -1;
+
+  fd = serialport_init(serial_port, baudrate);
+  if(fd == -1) {
+    fprintf(stderr, "couldn't open serial port %s\n", serial_port);
+    return -1;
+  } else {
+    fprintf(stderr, "Openning serial port: %s ...\n", serial_port);
+  }
+
+  serialport_flush(fd); // take 2 seconds
+  fprintf(stderr, "Serial port ready!\n");
+
+  return fd;
+
+}
+
+int serial_readline(int fd, char* buf, int buf_max)
+{
+  if( fd == -1 ){
+    perror("serial port not opened");
+    return -1;
+  }
+
+  memset(buf, 0, buf_max);
+
+  do {
+     serialport_read_until(fd, buf, SERIAL_EOL_CHAR, buf_max, SERIAL_TIMEOUT);
+  } while( buf[0] == '\n' );
+
+  // serialport_read_until(fd, buf, SERIAL_EOL_CHAR, buf_max, SERIAL_TIMEOUT);
+
+  return 0;
+}
+
+int serial_writeline(int fd, char* buf)
+{
+  if(buf[strlen(buf)-1] != SERIAL_EOL_CHAR){
+    //append EOL to buf
+    int len = strlen(buf);
+    buf[len] = SERIAL_EOL_CHAR;
+    buf[len+1] = '\0';
+  }
+
+  serialport_write(fd, buf);
+
+  return 0;
+}
+
+//For testing only
+void test_adruino_serial(void)
+{
+  int   fd;
+  char  buf[MAX_SERIAL_BUFFER];
+
+  fd = start_serial_inferface(DEFAULT_BAUDRATE, DEFAULT_SERIAL_PORT);
+
+  while(1){
+    fprintf(stderr, "[E2 Agent]: ");
+    fgets(buf, MAX_SERIAL_BUFFER, stdin);
+
+    // serialport_write(fd, "hello\n");
+    //serialport_write(fd, buf);
+    serial_writeline(fd, buf);
+
+    serial_readline(fd, buf, MAX_SERIAL_BUFFER);
+    fprintf(stderr, "[Adruino] %s", buf);
+  }
+
+  return;
+}
diff --git a/e2sim/e2apv1sim/test/Pendulum/Serial/adruino_serial.h b/e2sim/e2apv1sim/test/Pendulum/Serial/adruino_serial.h
new file mode 100644
index 0000000..c061f8a
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/Serial/adruino_serial.h
@@ -0,0 +1,40 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+#ifndef ADRUINO_SERIAL_H
+#define ADRUINO_SERIAL_H
+
+#include "arduino-serial-lib.h"
+
+#define MAX_SERIAL_BUFFER       256
+#define DEFAULT_SERIAL_PORT     "/dev/ttyACM0"
+#define DEFAULT_BAUDRATE        115200
+#define SERIAL_EOL_CHAR         '\n'
+#define SERIAL_TIMEOUT          5000 //ms
+
+int start_serial_inferface(int baudrate, char *serial_port);
+
+int serial_readline(int fd, char* buf, int buf_max);
+
+int serial_writeline(int fd, char* buf);
+
+//For testing only
+void test_adruino_serial(void);
+
+#endif
diff --git a/e2sim/e2apv1sim/test/Pendulum/Serial/arduino-serial-lib.c b/e2sim/e2apv1sim/test/Pendulum/Serial/arduino-serial-lib.c
new file mode 100644
index 0000000..73cac84
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/Serial/arduino-serial-lib.c
@@ -0,0 +1,170 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+//
+// arduino-serial-lib -- simple library for reading/writing serial ports
+//
+// 2006-2013, Tod E. Kurt, http://todbot.com/blog/
+//
+
+#include "arduino-serial-lib.h"
+
+#include <stdio.h>    // Standard input/output definitions
+#include <unistd.h>   // UNIX standard function definitions
+#include <fcntl.h>    // File control definitions
+#include <errno.h>    // Error number definitions
+#include <termios.h>  // POSIX terminal control definitions
+#include <string.h>   // String function definitions
+#include <sys/ioctl.h>
+
+// uncomment this to debug reads
+//#define SERIALPORTDEBUG
+
+// takes the string name of the serial port (e.g. "/dev/tty.usbserial","COM1")
+// and a baud rate (bps) and connects to that port at that speed and 8N1.
+// opens the port in fully raw mode so you can send binary data.
+// returns valid fd, or -1 on error
+int serialport_init(const char* serialport, int baud)
+{
+    struct termios toptions;
+    int fd;
+
+    //fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
+    fd = open(serialport, O_RDWR | O_NONBLOCK );
+
+    if (fd == -1)  {
+        perror("serialport_init: Unable to open port s");
+        return -1;
+    }
+
+    //int iflags = TIOCM_DTR;
+    //ioctl(fd, TIOCMBIS, &iflags);     // turn on DTR
+    //ioctl(fd, TIOCMBIC, &iflags);    // turn off DTR
+
+    if (tcgetattr(fd, &toptions) < 0) {
+        perror("serialport_init: Couldn't get term attributes");
+        return -1;
+    }
+    speed_t brate = baud; // let you override switch below if needed
+    switch(baud)
+    {
+        case 4800:   brate=B4800;   break;
+        case 9600:   brate=B9600;   break;
+      #ifdef B14400
+          case 14400:  brate=B14400;  break;
+      #endif
+          case 19200:  brate=B19200;  break;
+      #ifdef B28800
+          case 28800:  brate=B28800;  break;
+      #endif
+        case 38400:  brate=B38400;  break;
+        case 57600:  brate=B57600;  break;
+        case 115200: brate=B115200; break;
+    }
+    cfsetispeed(&toptions, brate);
+    cfsetospeed(&toptions, brate);
+
+    // 8N1
+    toptions.c_cflag &= ~PARENB;
+    toptions.c_cflag &= ~CSTOPB;
+    toptions.c_cflag &= ~CSIZE;
+    toptions.c_cflag |= CS8;
+    // no flow control
+    toptions.c_cflag &= ~CRTSCTS;
+
+    //toptions.c_cflag &= ~HUPCL; // disable hang-up-on-close to avoid reset
+
+    toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
+    toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
+
+    toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
+    toptions.c_oflag &= ~OPOST; // make raw
+
+    // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
+    toptions.c_cc[VMIN]  = 0;
+    toptions.c_cc[VTIME] = 0;
+    //toptions.c_cc[VTIME] = 20;
+
+    tcsetattr(fd, TCSANOW, &toptions);
+    if( tcsetattr(fd, TCSAFLUSH, &toptions) < 0) {
+        perror("init_serialport: Couldn't set term attributes");
+        return -1;
+    }
+
+    return fd;
+}
+
+//
+int serialport_close( int fd )
+{
+    return close( fd );
+}
+
+//
+int serialport_writebyte( int fd, uint8_t b)
+{
+    int n = write(fd,&b,1);
+    if( n!=1)
+        return -1;
+    return 0;
+}
+
+//
+int serialport_write(int fd, const char* str)
+{
+    int len = strlen(str);
+    int n = write(fd, str, len);
+    if( n!=len ) {
+        perror("serialport_write: couldn't write whole string\n");
+        return -1;
+    }
+    return 0;
+}
+
+//
+int serialport_read_until(int fd, char* buf, char until, int buf_max, int timeout)
+{
+    char b[1];  // read expects an array, so we give it a 1-byte array
+    int i=0;
+    do {
+        int n = read(fd, b, 1);  // read a char at a time
+        if( n==-1) return -1;    // couldn't read
+        if( n==0 ) {
+            usleep( 1 * 1000 );  // wait 1 msec try again
+            timeout--;
+            if( timeout==0 ) return -2;
+            continue;
+        }
+#ifdef SERIALPORTDEBUG
+        fprintf(stderr, "serialport_read_until: i=%d, n=%d b='%c'\n",i,n,b[0]); // debug
+#endif
+        buf[i] = b[0];
+        i++;
+    } while( b[0] != until && i < buf_max && timeout>0 );
+
+    buf[i] = 0;  // null terminate the string
+    return 0;
+}
+
+//
+int serialport_flush(int fd)
+{
+    sleep(2); //required to make flush work, for some reason
+    return tcflush(fd, TCIOFLUSH);
+}
diff --git a/e2sim/e2apv1sim/test/Pendulum/Serial/arduino-serial-lib.h b/e2sim/e2apv1sim/test/Pendulum/Serial/arduino-serial-lib.h
new file mode 100644
index 0000000..74fe592
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/Serial/arduino-serial-lib.h
@@ -0,0 +1,39 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+//
+// arduino-serial-lib -- simple library for reading/writing serial ports
+//
+// 2006-2013, Tod E. Kurt, http://todbot.com/blog/
+//
+
+
+#ifndef __ARDUINO_SERIAL_LIB_H__
+#define __ARDUINO_SERIAL_LIB_H__
+
+#include <stdint.h>   // Standard types
+
+int serialport_init(const char* serialport, int baud);
+int serialport_close(int fd);
+int serialport_writebyte( int fd, uint8_t b);
+int serialport_write(int fd, const char* str);
+int serialport_read_until(int fd, char* buf, char until, int buf_max,int timeout);
+int serialport_flush(int fd);
+
+#endif
diff --git a/e2sim/e2apv1sim/test/Pendulum/demo_setup.txt b/e2sim/e2apv1sim/test/Pendulum/demo_setup.txt
new file mode 100644
index 0000000..03cd1d6
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/demo_setup.txt
@@ -0,0 +1,14 @@
+# DEMO March 20, 2019

+  ## E2 Agent Laptop with IP x.x.x.x

+  - Connect to adruino via USB

+  - Connect to E2 Termination Desktop via Ethernet

+  - Run E2 Agent: $E2SIM_DIR/build/e2sim x.x.x.x

+

+  ## E2 Termination Desktop

+  - Run xApp

+    cd $E2SIM_DIR/rmr_interface/tests/pendulum_xapp

+    bash run_receiver

+

+  - Run E2 Termination

+    cd $E2SIM_DIR

+    ./build_and_run_e2sim

diff --git a/e2sim/e2apv1sim/test/Pendulum/e2sim_closedloop.c b/e2sim/e2apv1sim/test/Pendulum/e2sim_closedloop.c
new file mode 100644
index 0000000..613ce57
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/e2sim_closedloop.c
@@ -0,0 +1,263 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+#include <string.h>
+#include <assert.h>
+#include <unistd.h>             //for close()
+#include <arpa/inet.h>  //for inet_ntop()
+
+#include "e2sim_defs.h"
+#include "e2sim_sctp.h"
+#include "x2ap_message_handler.h"
+
+//OSN 2019
+#include "Pendulum_asn_codec.h"
+#include "adruino_serial.h"
+
+//rmr
+#include <errno.h>
+#include <sys/epoll.h>
+#include <rmr/rmr.h>
+#include "rmr_wrapper.h"
+
+static void pendulum_control_E2_agent(int client_fd)
+{
+ fprintf(stderr, "--------------------------------------\n");
+ fprintf(stderr, "E2 AGENT - START PENDULUM CONTROL\n");
+ fprintf(stderr, "--------------------------------------\n");
+
+ uint8_t   *buffer;
+ uint32_t  len;
+ clock_t   begin;
+ double    rtt; //ms
+
+ uint8_t recv_buf[MAX_SCTP_BUFFER];
+ int     recv_len;
+
+ double angle;
+ double torque;
+ long   sqn;
+ int    count = 0;
+
+ //serial
+ // int serial_fd;
+ char serial_buffer[MAX_SERIAL_BUFFER];
+ // serial_fd = start_serial_inferface(DEFAULT_BAUDRATE, DEFAULT_SERIAL_PORT);
+ //
+ // char *delay_str_prev = "$0#\n";
+ // //char *delay_str_new;
+ //
+ // int MSG_NUM = 10;
+ //
+ // //Always start with 0 delay
+ // serialport_write(serial_fd, "$0#\n");
+
+ // for(int i = 0; i < MSG_NUM; i++)
+ while(1)
+ {
+
+   fprintf(stderr, "----------------\n");
+   count += 1;
+   buffer  = NULL;
+   len     = 0;
+
+   // //1.Read from serial
+   // serial_readline(serial_fd, serial_buffer, MAX_SERIAL_BUFFER);
+   // if(serial_buffer[0] == '\n')
+   // {
+   //   //fprintf(stderr, "RECEIVED EOL\n");
+   //   continue;
+   // }
+   // //fprintf(stderr, "[Adruino] %s", serial_buffer);
+   usleep(5*1000);
+
+   snprintf(serial_buffer, sizeof(serial_buffer)-1, "E2 AGENT PING");
+
+   begin = clock();
+
+   //2. Encode pendulum angle to ASN1 message
+
+   len = pendulum_create_asn_msg(&buffer, 0, 0, 0, serial_buffer);
+
+   //3. Send ASN1 message to socket
+   if(sctp_send_to_socket(client_fd, buffer, (size_t)len) > 0){
+     fprintf(stderr, "Sent ASN1 message to E2 Termination\n");
+   }
+
+   // 4. Receive response from E2 Termination
+   memset(recv_buf, 0, sizeof(recv_buf));
+   recv_len = 0;
+   recv_len = recv(client_fd, &recv_buf, sizeof(recv_buf), 0);
+   if(recv_len == -1)
+   {
+     perror("recv()");
+     return;
+   }
+
+   char *recv_str;
+   recv_str = pendulum_get_strval(recv_buf, recv_len);
+   fprintf(stderr, "Received response message #%d from xApp: %s\n", count, recv_str);
+
+   // 5. TODO: Send response to serial
+   // Compare if delay has changed:
+   // if(strcmp(delay_str_prev, recv_str) != 0) {
+   //   serial_writeline(serial_fd, recv_str);
+   // }
+
+   //serial_writeline(serial_fd, recv_str);
+
+   //Write to a file
+   FILE *f;
+   f = fopen("arduino_delay.txt", "w");
+   fprintf(f, "%s", recv_str);
+   fclose(f);
+
+   begin = clock() - begin;
+   rtt = 1000*((double)begin)/CLOCKS_PER_SEC; // in ms
+   fprintf(stderr, "E2Agent-RIC-E2Agent RTT = %f ms\n", rtt);
+
+ }
+
+ close(client_fd);
+}
+
+int main(int argc, char *argv[])
+{
+ fprintf(stderr, "E2 AGENT - PENDULUM CONTROL. Version %s\n", VERSION);
+
+ // char *recv_str = "9";
+ // int delay;
+ //
+ // printf("delay = %d\n", atoi(recv_str));
+ //
+ // long delay = 22.5;
+ //
+ // printf("delay = %d\n", (int)delay);
+ // return 0;
+
+ // test_rmr(); return 0;
+ // test_adruino_serial(); return 0;
+
+ char* server_ip         = DEFAULT_SCTP_IP;
+ int server_port         = X2AP_SCTP_PORT;
+
+ int             server_fd;
+ int             client_fd;
+ struct sockaddr client_addr;
+ socklen_t       client_addr_size;
+
+ //read input
+ if(argc == 3) //user provided IP and PORT
+ {
+   server_ip = argv[1];
+   server_port = atoi(argv[2]);
+   if(server_port < 1 || server_port > 65535) {
+     fprintf(stderr, "Invalid port number (%d). Valid values are between 1 and 65535.\n"            , server_port);
+     return -1;
+   }
+ }
+ else if(argc == 2) //user provided only IP
+ {
+   server_ip = argv[1];
+ }
+ else if(argc == 1)
+ {
+   server_ip = DEFAULT_SCTP_IP;
+ }
+ else
+ {
+   fprintf(stderr, "Unrecognized option.\n");
+   fprintf(stderr, "Usage: %s [SERVER IP ADDRESS] [SERVER PORT]\n", argv[0]);
+   return -1;
+ }
+
+ server_fd = sctp_start_server(server_ip, server_port);
+
+ fprintf(stderr, "Waiting for connection...\n");
+ client_fd = accept(server_fd, &client_addr, &client_addr_size);
+ if(client_fd == -1){
+   perror("accept()");
+   close(client_fd);
+   return -1;
+ }
+
+ //Todo: retrieve client ip addr
+ struct sockaddr_in* client_ipv4 = (struct sockaddr_in*)&client_addr;
+ char client_ip_addr[INET_ADDRSTRLEN];
+ inet_ntop(AF_INET, &(client_ipv4->sin_addr), client_ip_addr, INET_ADDRSTRLEN);
+
+ fprintf(stderr, "New client connected from %s\n", client_ip_addr);
+
+ // while(1) //put while loop if want to receive from multiple clients
+ // {
+   uint8_t recv_buf[MAX_SCTP_BUFFER];
+   int     recv_len = 0;
+
+   memset(recv_buf, 0, sizeof(recv_buf));
+
+   fprintf(stderr, "------------------------\n");
+   recv_len = recv(client_fd, &recv_buf, sizeof(recv_buf), 0);
+   if(recv_len == -1)
+   {
+     perror("recv()");
+     return -1;
+   }
+   else if(recv_len == 0)
+   {
+     fprintf(stderr, "\nConnection from %s closed by remote peer\n", client_ip_addr);
+     if(close(client_fd) == -1)
+     {
+       perror("close");
+     }
+     return -1;
+   }
+
+   //fprintf(stderr, "Received a message of size %d\n", recv_len);
+
+   //TODO: check PPID here before calling x2ap handler
+
+   sctp_data_t response = {NULL, 0};
+   x2ap_eNB_handle_message(recv_buf, recv_len, &response);
+
+   //=======================================================================
+   //reply to client
+   assert(response.data != NULL);
+   if(sctp_send_to_socket(client_fd, response.data, (size_t)response.len) > 0){
+     fprintf(stderr, "Sent X2 SETUP RESPONSE \n");
+   } else{
+     perror("send to socket");
+     return -1;
+   }
+
+   fprintf(stderr, "X2 Setup Completed \n");
+
+   //=========================================================================
+   // Pendulum interaction
+   // Send pendulum state to E2 Termination and receive response
+   pendulum_control_E2_agent(client_fd);
+ // } //end while
+
+ close(client_fd);
+
+ return 0;
+}
diff --git a/e2sim/e2apv1sim/test/Pendulum/e2sim_serial.c b/e2sim/e2apv1sim/test/Pendulum/e2sim_serial.c
new file mode 100644
index 0000000..33fa0ba
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/e2sim_serial.c
@@ -0,0 +1,56 @@
+/*
+ *
+ * Copyright 2019 AT&T Intellectual Property
+ * Copyright 2019 Nokia
+ *
+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+#include <string.h>
+#include <assert.h>
+#include "e2sim_defs.h"
+#include <unistd.h>
+
+#include "adruino_serial.h"
+
+int main(int argc, char *argv[])
+{
+  int   fd;
+  char  buf[MAX_SERIAL_BUFFER];
+
+  fd = start_serial_inferface(DEFAULT_BAUDRATE, DEFAULT_SERIAL_PORT);
+
+  while(1)
+  {
+    //usleep(5*1000); //sleep 5ms between read
+    sleep(1);
+
+    FILE *f;
+    f = fopen("arduino_delay.txt", "r");
+    assert(f);
+
+    fread(buf, 1, sizeof(buf), f);
+
+    printf("delay content = %s \n", buf);
+
+    serial_writeline(fd, buf);
+
+    fclose(f);
+  }
+
+  return 0;
+}
diff --git a/e2sim/e2apv1sim/test/Pendulum/e2sim_test_client.c b/e2sim/e2apv1sim/test/Pendulum/e2sim_test_client.c
new file mode 100644
index 0000000..b178ff7
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/e2sim_test_client.c
@@ -0,0 +1,378 @@
+/*

+ *

+ * Copyright 2019 AT&T Intellectual Property

+ * Copyright 2019 Nokia

+ *

+ * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0

+ *

+ * 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.

+ *

+ */

+

+#include <stdio.h>

+#include <string.h>

+#include <time.h>

+#include <unistd.h>		//for close()

+#include <stdlib.h>

+#include <sys/socket.h>

+#include <netinet/in.h>

+#include <netinet/sctp.h>

+#include <arpa/inet.h>	//for inet_ntop()

+#include <assert.h>

+

+#include "e2sim_defs.h"

+#include "e2sim_sctp.h"

+#include "x2ap_message_handler.h"

+

+#include "x2ap_generate_messages.h"

+

+//OSN 2019

+#include "Pendulum_asn_codec.h"

+

+//rmr

+#include <errno.h>

+#include <sys/epoll.h>

+#include <rmr/rmr.h>

+#include "rmr_wrapper.h"

+

+//time

+#include <sys/time.h>

+

+//these are the metrics being sent to the a1 mediator

+int ave_ric_rtt_last_epoch=0;

+int ave_msg_rate_last_epoch=0;

+int ave_pendulum_msg_rate_last_epoch=0;

+

+int total_rtt_current_epoch=0;

+int total_messages_current_epoch=0;

+int total_pendulum_messages_current_epoch=0;

+

+int total_rtt_entries_current_epoch=0;

+

+int epoch_duration = 1;//in seconds

+

+long rtt_epoch_start_time =0;

+

+long msg_rate_epoch_start_time =0;

+long pendulum_msg_rate_epoch_start_time = 0;

+

+long current_timestamp_in_us(){

+	struct timeval currentTime;

+	gettimeofday(&currentTime, NULL);

+	return currentTime.tv_sec * (int)1e6 + currentTime.tv_usec;

+}

+void update_rtt_metrics(long rtt){//called every time there is a new rtt measurement

+	if(rtt_epoch_start_time == 0)

+		rtt_epoch_start_time = current_timestamp_in_us(); //start of a new epoch

+

+	total_rtt_current_epoch = total_rtt_current_epoch+rtt;

+	total_rtt_entries_current_epoch++;

+

+	if((current_timestamp_in_us() - rtt_epoch_start_time) > (epoch_duration*1000000)){//an epoch has passed

+		ave_ric_rtt_last_epoch = total_rtt_current_epoch/total_rtt_entries_current_epoch;

+		total_rtt_current_epoch =0;

+		rtt_epoch_start_time = 0;

+	}

+}

+

+void update_msg_rate_metrics(){

+	if(msg_rate_epoch_start_time == 0)

+		msg_rate_epoch_start_time= current_timestamp_in_us(); //start of a new epoch

+	total_messages_current_epoch++;

+	if((current_timestamp_in_us() - msg_rate_epoch_start_time) > (epoch_duration*1000000)){//an epoch has passed

+		ave_msg_rate_last_epoch = total_messages_current_epoch;

+		total_messages_current_epoch =0;

+		msg_rate_epoch_start_time =0;

+	}

+}

+

+void update_pendulum_control_rate()

+{

+	if(pendulum_msg_rate_epoch_start_time == 0)

+		pendulum_msg_rate_epoch_start_time = current_timestamp_in_us(); //start of a new epoch

+	total_pendulum_messages_current_epoch++;

+

+	if((current_timestamp_in_us() - pendulum_msg_rate_epoch_start_time) > (epoch_duration*1000000)){//an epoch has passed

+		ave_pendulum_msg_rate_last_epoch = total_pendulum_messages_current_epoch;

+		total_pendulum_messages_current_epoch = 0;

+		pendulum_msg_rate_epoch_start_time = 0;

+	}

+

+}

+

+void send_metrics_to_a1_med(struct rmr_context *rmr_c){

+	 int mtype=103;

+	 char* metrics= malloc(1024);

+	 int time_int = current_timestamp_in_us()/1000000;

+	 //int ave_msg_rate_last_epoch=500;

+	 snprintf(metrics, 1024, "{%s:%d, %s:%d, %s:%d, %s:%d}", \

+	 									"\"latency\"", ave_ric_rtt_last_epoch/1000, \

+										"\"ricload\"",ave_msg_rate_last_epoch, \

+										"\"load\"",ave_pendulum_msg_rate_last_epoch, \

+										"\"time\"",time_int);

+	 rmr_send_wrapper(rmr_c, mtype, metrics);

+	 printf("Sent message of type:%d to a1_med with content:%s\n",mtype,metrics);

+}

+

+void forward_to_load_consumer(struct rmr_context *rmr_c){//the content does not matter

+	 int mtype=105;

+	 char* load_message="dummy load";

+	 rmr_send_wrapper(rmr_c, mtype, load_message);

+	 printf("Sent message of type:%d to load consumer with content:%s\n",mtype,load_message);

+}

+

+static void pendulum_control_E2_Termination(int client_fd)

+{

+  printf("--------------------------------------\n");

+  printf("E2 TERMINATION - START PENDULUM CONTROL\n");

+  printf("--------------------------------------\n");

+

+  uint8_t *send_buffer;

+  uint8_t recv_buffer[1024];

+  uint32_t send_len;

+  uint32_t recv_len;

+

+  clock_t begin = clock();

+  double rtt;

+  double rtt_stats[100000];

+  long   recv_count = 0;

+  long   fail_count = -1; //ignore the first message (see adruino code)

+

+  long   sqn;

+  int    count = 0;

+

+  //=================================

+

+  //Setup context

+  struct rmr_context *rmr_c; //obtain our enhanced rmr_context

+  int 	mtype = 0;						// we can loop through several message types

+  char*	lport = "43086";				// default listen port

+  long	rcount = 0;						// number of acks received

+

+  if( (eparm = getenv( "E2TERM_RMR_RCV_PORT" )) != NULL ) {

+                lport = strdup( eparm );

+  }

+

+  rmr_c = rmr_init_wrapper(lport);

+

+  while( ! rmr_ready( rmr_c->mrc ) ) {

+    fprintf( stderr, "<TEST> waiting for RMR to indicate ready\n" );

+    sleep( 1 );

+  }

+  fprintf( stderr, "[OK]   initialisation complete\n" );

+

+

+  //==================================

+  long loop_start_time = 0;

+  while(1){

+    printf("----------------\n");

+    count += 1;

+    loop_start_time = current_timestamp_in_us();

+    //0. Receiving ASN message from E2 Agent

+    memset(recv_buffer, 0, sizeof(recv_buffer));

+    printf("Time to receive asn message after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+    recv_len = 0;

+

+    printf(" 1Time to receive asn message after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+    //long time_of_message_from_e2agent = current_timestamp_in_us();

+

+    if((recv_len = recv(client_fd, &recv_buffer, sizeof(recv_buffer), 0)) == -1) {

+        perror("recv");

+        return;

+    }

+

+    long time_of_message_from_e2agent = current_timestamp_in_us();

+

+    printf(" 2Time to receive asn message after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+    if(recv_len == 0) {

+        rmr_close_wrapper(rmr_c);

+

+        printf("Connection from closed by remote peer.\n");

+        if(close(client_fd) == -1) {

+            perror("close");

+        }

+        return;

+    }

+

+    printf(" 3Time to receive asn message after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+    // begin = clock() - begin;

+    // rtt = 1000*((double)begin)/CLOCKS_PER_SEC; // in ms

+    //printf("E2Term-Adruino-E2Term = %f ms\n", rtt);

+

+    //2. Decode ASN message and Extract pendulum angle

+    char *recv_str;

+    recv_str = pendulum_get_strval(recv_buffer, recv_len);

+    // if( (strcmp(recv_str, "-1") == 0) || (strcmp(recv_str, "") == 0) )

+

+    if(strcmp(recv_str, "\n") == 0)

+    {

+      printf("RECEIVED EOL\n");

+    }

+

+    printf(" 4Time to receive asn message after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+    // if(atof(recv_str) <= 0)

+    // {

+    //   printf("FAILLLLLL\n");

+    //   fail_count += 1;

+    // }

+    // else {

+    //   rtt_stats[recv_count] = atof(recv_str);

+    //   recv_count++;

+    // }

+

+    printf("Time to receive angle message from arduino after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+    printf("Received message #%d from Adruino: %s\n", count, recv_str);

+    //printf("Last reported RTT (Adruino-RIC-Adruino): %f ms, fail_count = %ld\n",

+    // atof(recv_str)/1000, fail_count);

+

+    // 3. [BHARATH] send_to_xApp(&pendulum_state, &response)

+    // while(1) {

+    // usleep( 10 );

+    // char* message = "foo 111";

+    char reply[1024];

+    int got_pend_control=0;

+    rmr_send_wrapper(rmr_c, mtype, recv_str );

+    printf("Sent message of type:%d to pendulum xApp with content:%s\n",mtype,recv_str);

+    long angle_receive_time =0;

+    while (got_pend_control == 0){

+	    if(rmr_poll_for_message(rmr_c) == 1) {

+		    update_msg_rate_metrics();

+		    switch(rmr_c->rbuf->mtype) {

+			    case 33 :

+    		    	            angle_receive_time = current_timestamp_in_us();

+				    got_pend_control=1;

+				    update_pendulum_control_rate(); //add this

+				    strcpy(reply,rmr_c->rbuf->payload);

+				    printf("Received control message from pendulum xapp with message type: %d and content %s\n",rmr_c->rbuf->mtype, reply);

+				    break;

+			    case 102 :

+				    printf("Received METRIC request from A1 mediator with message type: %d and content %s\n",rmr_c->rbuf->mtype,rmr_c->rbuf->payload);

+				    send_metrics_to_a1_med(rmr_c);

+				    break;

+			    case 104 :

+				    printf("***************************Received load from load generator****************************");

+				    forward_to_load_consumer(rmr_c);

+				    break;

+			    default :

+				    continue;

+		    }

+	    }

+

+    }

+    printf("Time to receive control message from xapp after starting main loop: %ld microseconds \n",current_timestamp_in_us() - loop_start_time);

+//    snprintf(reply, 1024, "$%d#\n", (int)ave_ric_rtt_last_epoch/1000);

+    send_len = pendulum_create_asn_msg(&send_buffer, 0, 0, 0, reply);

+    printf("Time to create asn message after receiving angle: %ld microseconds\n",current_timestamp_in_us() - angle_receive_time);

+    begin = clock();

+

+    //6. Send ASN1 message to socket

+    if(sctp_send_to_socket(client_fd, send_buffer, (size_t)send_len) > 0){

+      printf("Sent ASN1 response to E2 Agent\n");

+    }

+    long time_of_reply_to_e2agent = current_timestamp_in_us();

+    printf("Time to send asn message after receiving angle: %ld microseconds \n",current_timestamp_in_us() - angle_receive_time);

+    long rtt = (time_of_reply_to_e2agent - time_of_message_from_e2agent);

+    ave_ric_rtt_last_epoch = rtt;

+    printf("RIC RTT is %lf milliseconds\n", rtt/1000.0);

+    //update_rtt_metrics(rtt);

+  }

+

+  rmr_close_wrapper(rmr_c);

+

+}

+

+int main(int argc, char* argv[])

+{

+  // test_rmr(); return 0;

+

+  const char* server_ip   = DEFAULT_SCTP_IP;

+  int server_port         = X2AP_SCTP_PORT;

+

+  //read input

+  if(argc == 3) //user provided IP and PORT

+  {

+    server_ip = argv[1];

+    server_port = atoi(argv[2]);

+    if(server_port < 1 || server_port > 65535) {

+      printf("Invalid port number (%d). Valid values are between 1 and 65535.\n", server_port);

+      return 1;

+    }

+  }

+  else if(argc == 2) //user provided only IP

+  {

+    server_ip = argv[1];

+  }

+  else if(argc == 1)

+  {

+    server_ip = DEFAULT_SCTP_IP;

+  }

+  else

+  {

+    printf("Unrecognized option.\n");

+    printf("Usage: %s [SERVER IP ADDRESS] [SERVER PORT]\n", argv[0]);

+    return 0;

+  }

+

+  int client_fd;

+  client_fd = sctp_start_client(server_ip, server_port);

+

+  uint8_t *buffer;

+  uint32_t  len;

+

+  //Note: put a while(1) loop here if want client to stay

+  // for(int i = 0; i < 3; i++)

+  // {

+    buffer = NULL;

+    len = 0;

+

+    printf("------------------------\n");

+    clock_t begin;

+    begin = clock();

+

+    //Create pdu for x2 message and send to socket

+    len = x2ap_generate_x2_setup_request(&buffer);

+    if(sctp_send_to_socket(client_fd, buffer, (size_t)len) > 0){

+      printf("Sent X2 SETUP REQUEST\n");

+    }

+

+    //=======================================================================

+    //printf("waiting for server response\n");

+    uint8_t recv_buf[MAX_SCTP_BUFFER];

+    int recv_len = 0;

+

+    //sctp_recv_from_socket(client_fd, recv_buf, sizeof(recv_buf));

+    memset(recv_buf, 0, sizeof(recv_buf));

+    recv_len = recv(client_fd, &recv_buf, sizeof(recv_buf), 0);

+    if(recv_len == -1)

+    {

+      perror("recv()");

+      return -1;

+    }

+

+    //printf("Received a message of size %d\n", recv_len);

+    x2ap_eNB_handle_message(recv_buf, recv_len, NULL);

+

+    begin = clock() - begin;

+    double time_taken = 1000*((double)begin)/CLOCKS_PER_SEC; // in ms

+    printf("Close-loop time: %f ms \n", time_taken);

+    printf("X2 Setup Completed \n");

+

+  // } //end iteration

+

+  //=========================================================================

+  // Pendulum interaction

+  // Receive pendulum state from E2 Agent and send response

+  pendulum_control_E2_Termination(client_fd);

+

+  close(client_fd);

+

+  return 0;

+}

diff --git a/e2sim/e2apv1sim/test/Pendulum/e2termination_test.cpp b/e2sim/e2apv1sim/test/Pendulum/e2termination_test.cpp
new file mode 100644
index 0000000..b65ac65
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/e2termination_test.cpp
@@ -0,0 +1,223 @@
+/*****************************************************************************
+#                                                                            *
+# Copyright 2019 AT&T Intellectual Property                                  *
+# Copyright 2019 Nokia                                                       *
+#                                                                            *
+# Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0                            *
+#                                                                            *
+# 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.                                             *
+#                                                                            *
+******************************************************************************/
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string>
+#include <iostream>
+
+
+#include "e2sim_defs.h"
+#include "asn_e2ap.hpp"
+#include "e2sim_sctp.hpp"
+#include "e2ap_message_handler.hpp"
+#include "e2ap_asn_codec.hpp"
+
+using namespace std;
+
+void test_send_X2Setup(int &client_fd)
+{
+    //Create X2SetupRequest message
+    e2ap_pdu_t* pdu = new_e2ap_pdu();
+    eNB_config cfg;
+
+    e2ap_create_X2SetupRequest(pdu, cfg);
+    e2ap_print_pdu(pdu);
+
+    //Encode into buffer
+    sctp_buffer_t data;
+    e2ap_encode_pdu(pdu, data.buffer, sizeof(data.buffer), data.len);
+
+    //Send to sctp
+    sctp_send_data(client_fd, data);
+    LOG_I("[SCTP] Sent X2 SETUP REQUEST");
+
+    //wait to receive response
+    sctp_buffer_t recv_buf;
+    if(sctp_receive_data(client_fd, recv_buf) > 0)
+    {
+      LOG_I("[SCTP] Received new data of size %d", recv_buf.len);
+      e2ap_handle_sctp_data(client_fd, recv_buf);
+    }
+
+  }
+
+void test_send_ENDCX2Setup(int &client_fd)
+{
+  //Create ENDCX2SetupRequest message
+  e2ap_pdu_t* pdu = new_e2ap_pdu();
+  eNB_config cfg;
+
+  e2ap_create_ENDCX2SetupRequest(pdu, cfg);
+  e2ap_print_pdu(pdu);
+
+  //Encode into buffer
+  sctp_buffer_t data;
+  e2ap_encode_pdu(pdu, data.buffer, sizeof(data.buffer), data.len);
+
+  //Send to sctp
+  sctp_send_data(client_fd, data);
+  LOG_I("[SCTP] Sent ENDC X2 SETUP REQUEST");
+
+  //wait to receive response
+  sctp_buffer_t recv_buf;
+  if(sctp_receive_data(client_fd, recv_buf) > 0)
+  {
+    LOG_I("[SCTP] Received new data of size %d", recv_buf.len);
+    e2ap_handle_sctp_data(client_fd, recv_buf);
+  }
+
+}
+
+void test_send_RICSubscriptionRequest(int &client_fd)
+{
+  LOG_I("Test RIC SUBSCRIPTION");
+
+  /* Create RIC SUBSCRITION REQUEST */
+  e2ap_pdu_t* pdu = new_e2ap_pdu();
+
+  RICsubscription_params_t params;
+  params.request_id = 2;
+  params.seq_number = 200;
+  params.ran_func_id = 0;
+  params.event_trigger_def = "hello world";
+
+  RIC_action_t action1(1, RICactionType_report);
+  // RIC_action_t action2(3, RICactionType_insert);
+  // RIC_action_t action3(5, RICactionType_insert);
+  // RIC_action_t action4(7, RICactionType_insert);
+  params.actionList.push_back(action1);
+  // params.actionList.push_back(action2);
+  // params.actionList.push_back(action3);
+  // params.actionList.push_back(action4);
+
+  e2ap_create_RICsubscriptionRequest(pdu, params);
+  e2ap_print_pdu(pdu);
+
+  //Encode into buffer
+  sctp_buffer_t data;
+  e2ap_encode_pdu(pdu, data.buffer, sizeof(data.buffer), data.len);
+
+  //Send to sctp
+  sctp_send_data(client_fd, data);
+  LOG_I("[SCTP] Sent RIC SUBSCRIPTION REQUEST");
+
+  //wait to receive response and indication (if any)
+  sctp_buffer_t recv_buf;
+  LOG_I("[SCTP] Waiting for SCTP data");
+  while(1)
+  {
+    if(sctp_receive_data(client_fd, recv_buf) > 0)
+    {
+      LOG_I("[SCTP] Received new data of size %d", recv_buf.len);
+      e2ap_handle_sctp_data(client_fd, recv_buf);
+    }
+    else
+      break;
+
+  }
+
+
+  return;
+  //==========================================================================
+
+  //decode
+  e2ap_pdu_t* pdu2 = new_e2ap_pdu();
+
+  e2ap_decode_pdu(pdu2, data.buffer, data.len);
+
+  RICsubscription_params_t params2;
+  e2ap_parse_RICsubscriptionRequest(pdu2, params2);
+  printf("request_id = %d\n", params2.request_id);
+  printf("seq_number = %d\n", params2.seq_number);
+  printf("ran_func_id = %d\n", params2.ran_func_id);
+  printf("event = %s\n", &params2.event_trigger_def[0]);
+
+  for( auto const  &a : params.actionList)
+  {
+    printf("action id = %d, action type = %d\n", (int)a.action_id, (int)a.action_type);
+  }
+
+
+  LOG_I("================= RESPONSE ===========================");
+  for(size_t i = 0; i < params2.actionList.size(); i++)
+  {
+    //example logic: admit every other action
+    if(i%2 == 0) {
+      params2.actionList[i].isAdmitted = true;
+    } else {
+      params2.actionList[i].isAdmitted = false;
+      params2.actionList[i].notAdmitted_cause = RICcause_radioNetwork;
+      params2.actionList[i].notAdmitted_subCause = 5;
+    }
+  }
+
+  e2ap_pdu_t* res_pdu = new_e2ap_pdu();
+  e2ap_create_RICsubscriptionResponse(res_pdu, params2);
+
+  e2ap_print_pdu(res_pdu);
+
+  //Encode into buffer
+  sctp_buffer_t data_resp;
+  e2ap_encode_pdu(res_pdu, data_resp.buffer, sizeof(data_resp.buffer), data_resp.len);
+
+  LOG_I("================= FAILURE ===========================");
+  e2ap_pdu_t* fail_pdu = new_e2ap_pdu();
+
+  RICsubscription_params_t params3;
+  e2ap_parse_RICsubscriptionRequest(pdu2, params3);
+
+  for(size_t i = 0; i < params3.actionList.size(); i++)
+  {
+    params3.actionList[i].isAdmitted = false;
+    params3.actionList[i].notAdmitted_cause = RICcause_radioNetwork;
+    params3.actionList[i].notAdmitted_subCause = 5;
+  }
+
+  e2ap_create_RICsubscriptionFailure(fail_pdu, params3);
+  e2ap_print_pdu(fail_pdu);
+
+  //Encode into buffer
+  sctp_buffer_t data_fail;
+  e2ap_encode_pdu(fail_pdu, data_fail.buffer, sizeof(data_fail.buffer), data_fail.len);
+
+}
+
+int main(int argc, char* argv[]){
+
+  LOG_I("E2 Termination Test");
+
+  // test_send_RICSubscriptionRequest();
+  // return 0;
+
+  options_t ops = read_input_options(argc, argv);
+  int client_fd = sctp_start_client(ops.server_ip, ops.server_port);
+
+  //---------------------------------------------------
+  // test_send_X2Setup(client_fd);
+  test_send_ENDCX2Setup(client_fd);
+  test_send_RICSubscriptionRequest(client_fd);
+
+  //---------------------------------------------------
+
+  close(client_fd);
+  LOG_I("[SCTP] Connection closed.");
+
+  return 0;
+}
diff --git a/e2sim/e2apv1sim/test/Pendulum/pendulum.asn1 b/e2sim/e2apv1sim/test/Pendulum/pendulum.asn1
new file mode 100644
index 0000000..686d2c0
--- /dev/null
+++ b/e2sim/e2apv1sim/test/Pendulum/pendulum.asn1
@@ -0,0 +1,12 @@
+PendulumModule DEFINITIONS AUTOMATIC TAGS ::=

+

+BEGIN

+

+Pendulum ::= SEQUENCE {

+  sequence  INTEGER         OPTIONAL,

+  angle     REAL            OPTIONAL,

+  torque    REAL            OPTIONAL,

+  strval    PrintableString OPTIONAL

+}

+

+END