/*
 * Licensed to Open Source Radio Access Network(OS-RAN) Alliance and OAI
 * Software Alliance under one or more contributor license agreements. The
 * initial OpenXG series projects are derided from OAI projects, the files from
 * OAI projects are all in compliance with OAI Public License, Version 1.1.
 * codes and files developed from OpenXG projects and from OS-RAN Alliance
 * are all under OS-RAN licenses; you may not use this file except in compliance
 * with the license.  You may get a copy of the license at:
 *	http://www.openxg.org.cn/?falu_69.html
 * For more information about OpenXG, please contact:
 * contact@openxg.org.cn
 */



#ifndef _TEST_HPP_
#define _TEST_HPP_

#include <stdlib.h>

#include "OCTET_STRING.h"
#include "authentication_algorithms_with_5gaka.hpp"
#include "f12345.h"

// uint8_t key[16] = {0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f, 0xaa,
// 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc}; uint8_t key[16] = {0x03, 0x96,
// 0xeb, 0x31, 0x7b, 0x6d, 0x1c, 0x36, 0xf1, 0x9c, 0x1c, 0x84, 0xcd, 0x6f, 0xfd,
// 0x16}; uint8_t opc[16] = {0xcd, 0x63, 0xcb, 0x71, 0x95, 0x4a, 0x9f, 0x4e,
// 0x48, 0xa5, 0x99, 0x4e, 0x37, 0xa0, 0x2b, 0xaf}; uint8_t rand_[16] = {0x23,
// 0x55, 0x3c, 0xbe, 0x96, 0x37, 0xa8, 0x9d, 0x21, 0x8a, 0xe6, 0x4d, 0xae, 0x47,
// 0xbf, 0x35 }; uint8_t sqn[6] = {0xff, 0x9b, 0xb4, 0xd0, 0xb6, 0x07}; uint8_t
// amf[2] = {0xb9, 0xb9};

extern void print_buffer(const std::string app, const std::string commit,
                         uint8_t *buf, int len);

int ff1_test() {
#if 0
  uint8_t mac_a[8];
  Authentication_5gaka::f1(opc, key, rand_, sqn, amf, mac_a); 
 //f1star(key, rand_, sqn, amf, mac_a); 
  printf("mac_a: 0x");
  for(int i=0; i<8; i++)
    printf("%x", mac_a[i]);
  printf("\n");
#endif
#if 0
  Authentication_5gaka::RijndaelKeySchedule(key);
  uint8_t plain_text[16] = {0xee, 0x36, 0xf7, 0xcf, 0x03, 0x7d, 0x37, 0xd3, 0x69, 0x2f, 0x7f, 0x03, 0x99, 0xe7, 0x94, 0x9a};
  //uint8_t plain_text[16] = {0x93, 0xcc, 0x36, 0x40, 0xc5, 0xd6, 0xa5, 0x21, 0xd8, 0x12, 0x35, 0xbd, 0x08, 0x82, 0xbf, 0x0a};
  uint8_t output[16];
  Authentication_5gaka::RijndaelEncrypt(plain_text, output);
  printf("output: 0x");
  for(int i=0; i<16; i++)
    printf("%x", output[i]);
  printf("\n");
#endif
#if 0
  uint8_t op[16] = {0xcd, 0xc2, 0x02, 0xd5, 0x12, 0x3e, 0x20, 0xf6, 0x2b, 0x6d, 0x67, 0x6a, 0xc7, 0x2c, 0xb3, 0x18};
  uint8_t opc[16];
  Authentication_5gaka::ComputeOPc(key, op, opc);
  uint8_t res[8], ck[16], ik[16], ak[6];
  Authentication_5gaka::f2345(opc, key, rand_, res, ck, ik, ak);
  printf("res: 0x");
  for(int i=0; i<8; i++)
    printf("%x ", res[i]);
  printf("\n");
  printf("ak: 0x");
  for(int i=0; i<6; i++)
    printf("%x ", ak[i]);
  printf("\n");
  printf("ck: 0x");
  for(int i=0; i<16; i++)
    printf("%x ", ck[i]);
  printf("\n");
  printf("ik: 0x");
  for(int i=0; i<16; i++)
    printf("%x ", ik[i]);
  printf("\n");
  uint8_t akstar[6];
  Authentication_5gaka::f5star(opc, key, rand_, akstar);
  printf("akstar: 0x");
  for(int i=0; i<6; i++)
    printf("%x ", akstar[i]);
  printf("\n");
#endif
}

void string_to_utf8(const std::string &word,
                    std::vector<std::string> &characters) {
  int num = word.size();
  int i = 0;
  while (i < num) {
    int size = 1;
    if (word[i] & 0x80) {
      char temp = word[i];
      temp <<= 1;
      do {
        temp <<= 1;
        ++size;
      } while (temp & 0x80);
    }
    printf("size = %d", size);
    std::string subWord;
    subWord = word.substr(i, size);
    characters.push_back(subWord);
    i += size;
  }
}

#if 1

void kdf_test() {
  // string key = "00112233445566778899aabbccddeeff";
  // OCTET_STRING_t K;
  // OCTET_STRING_fromBuf(&K, key.c_str(), key.length());

  uint8_t Key[32] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
                     0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff};
  // uint8_t Key[16] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88,
  // 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}; uint8_t key[32] = {0x46, 0x5b,
  // 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f, 0xaa, 0x5f, 0x0a, 0x2e, 0xe2, 0x38,
  // 0xa6, 0xbc, 0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f, 0xaa, 0x5f,
  // 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc};
  uint8_t key[16] = {0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f,
                     0xaa, 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc};

  std::string msg = "k5G:mnc001.mcc110.3gppnetwork.org";
  // string msg = "k";
  OCTET_STRING_t M;
  OCTET_STRING_fromBuf(&M, msg.c_str(), msg.length());

  uint8_t O[16];

  Authentication_5gaka::kdf(Key, 32, M.buf, M.size, O, 16);
  // Authentication_5gaka::kdf(K.buf, K.size, M.buf, M.size, O, 16);
  printf("KDF test ...\n");
  for (int i = 0; i < 16; i++) printf("%x ", O[i]);
  printf("\n");
}

#endif

void rijndael_test() {
  printf("RijndaelKeySchedule testing ...\n");
  uint8_t key[16] = {0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f,
                     0xaa, 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc};
  uint8_t key1[16] = {0x03, 0x96, 0xeb, 0x31, 0x7b, 0x6d, 0x1c, 0x36,
                      0xf1, 0x9c, 0x1c, 0x84, 0xcd, 0x6f, 0xfd, 0x16};
  Authentication_5gaka::RijndaelKeySchedule(key1);
  uint8_t txt[16] = {0x93, 0xcc, 0x36, 0x40, 0xc5, 0xd6, 0xa5, 0x21,
                     0xd8, 0x12, 0x35, 0xbd, 0x08, 0x82, 0xbf, 0x0a};
  uint8_t txt_enc[16];
  Authentication_5gaka::RijndaelEncrypt(txt, txt_enc);
  printf("encrypted text ...\n");
  for (int i = 0; i < 16; i++) printf("%x ", txt_enc[i]);
  printf("\n");
}

void f1_test() {
  uint8_t opc[16] = {0xcd, 0x63, 0xcb, 0x71, 0x95, 0x4a, 0x9f, 0x4e,
                     0x48, 0xa5, 0x99, 0x4e, 0x37, 0xa0, 0x2b, 0xaf};
  uint8_t rand[16] = {0x23, 0x55, 0x3c, 0xbe, 0x96, 0x37, 0xa8, 0x9d,
                      0x21, 0x8a, 0xe6, 0x4d, 0xae, 0x47, 0xbf, 0x35};
  uint8_t key[16] = {0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f,
                     0xaa, 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc};
  uint8_t sqn[6] = {0xff, 0x9b, 0xb4, 0xd0, 0xb6, 0x07};
  uint8_t amf[2] = {0xb9, 0xb9};
  uint8_t mac_a[8], mac_s[8];
  Authentication_5gaka::f1(opc, key, rand, sqn, amf, mac_a);
  Authentication_5gaka::f1star(opc, key, rand, sqn, amf, mac_s);
  printf("testing f1...\n");
  for (int i = 0; i < 8; i++) printf("%x ", mac_a[i]);
  printf("\n");
  for (int i = 0; i < 8; i++) printf("%x ", mac_s[i]);
  printf("\n");
}

void f2345_test() {
  // uint8_t opc[16] = {0xcd, 0x63, 0xcb, 0x71, 0x95, 0x4a, 0x9f, 0x4e, 0x48,
  // 0xa5, 0x99, 0x4e, 0x37, 0xa0, 0x2b, 0xaf}; uint8_t rand[16] = {0x23, 0x55,
  // 0x3c, 0xbe, 0x96, 0x37, 0xa8, 0x9d, 0x21, 0x8a, 0xe6, 0x4d, 0xae, 0x47,
  // 0xbf, 0x35}; uint8_t key[16] = {0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4,
  // 0x9f, 0xaa, 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc};
  uint8_t opc[16] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                     0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
  uint8_t key[16] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff};
  // uint8_t rand[16] = {0x5b, 0x2e, 0x1c, 0x24, 0x28, 0xc7, 0x56, 0x28, 0xe4,
  // 0x43, 0xb0, 0xac, 0x89, 0x5d, 0x5b, 0x73};
  uint8_t rand[16] = {0x72, 0x13, 0x34, 0xee, 0x3e, 0xf1, 0xcd, 0x1b,
                      0x10, 0xfc, 0xe3, 0x63, 0xb4, 0x52, 0xde, 0xaa};
  uint8_t res[8], ck[16], ik[16], ak[6];
  Authentication_5gaka::f2345(opc, key, rand, res, ck, ik, ak);
  printf("res: 0x\n");
  for (int i = 0; i < 8; i++) printf("%x", res[i]);
  printf("\n");
  printf("ak: 0x");
  for (int i = 0; i < 6; i++) printf("%x", ak[i]);
  printf("\n");
  printf("ck: 0x\n");
  for (int i = 0; i < 16; i++) printf("%x", ck[i]);
  printf("\n");
  printf("ik: 0x\n");
  for (int i = 0; i < 16; i++) printf("%x", ik[i]);
  printf("\n");
  uint8_t ak2[6];
  Authentication_5gaka::f5star(opc, key, rand, ak2);
  printf("ak2: 0x");
  for (int i = 0; i < 6; i++) printf("%x ", ak2[i]);
  printf("\n");
}

void res_test() {
  uint8_t rand[16] = {0x23, 0x55, 0x3c, 0xbe, 0x96, 0x37, 0xa8, 0x9d,
                      0x21, 0x8a, 0xe6, 0x4d, 0xae, 0x47, 0xbf, 0x35};
  // uint8_t key[32] = {0x46, 0x5b, 0x5c, 0xe8, 0xb1, 0x99, 0xb4, 0x9f, 0xaa,
  // 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc, 0x46, 0x5b, 0x5c, 0xe8, 0xb1,
  // 0x99, 0xb4, 0x9f, 0xaa, 0x5f, 0x0a, 0x2e, 0xe2, 0x38, 0xa6, 0xbc};
  uint8_t key[32] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
                     0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff};
  std::string snn = "5G:mnc001.mcc110.3gppnetwork.org";
  OCTET_STRING_t netName;
  OCTET_STRING_fromBuf(&netName, snn.c_str(), snn.length());
  uint8_t S[100];
  S[0] = 0x6B;
  memcpy(&S[1], netName.buf, netName.size);
  printf("size = %x", netName.size);
  S[1 + netName.size] = (netName.size & 0xff00) >> 8;
  S[2 + netName.size] = (netName.size & 0x00ff);
  // for(int i=0; i<16; i++)
  //  S[3+netName.size+i] = rand[i];
  // S[19+netName.size] = 0x00;
  // S[20+netName.size] = 0x10;
  uint8_t output[16];
  // Authentication_5gaka::kdf(key, 32, K.buf, K.size, output, 16);
  Authentication_5gaka::kdf(key, 32, S, 3 + netName.size, output, 16);
  printf("inputstring ...\n");
  // for(int i=0; i< 1; i++)
  for (int i = 0; i < 3 + netName.size; i++) printf("%c", S[i]);
  printf("\n");
  printf("key ...\n");
  for (int i = 0; i < 32; i++) printf("%x", key[i]);
  printf("\n");
  printf("out ...\n");
  for (int i = 0; i < 16; i++) printf("%x ", output[i]);
  printf("\n");
}

// #include "nas_algorithms.hpp"

// void nia1_test(){
//   uint32_t count = 0x38a6f056;
//   uint8_t key[16] = {0x2b, 0xd6, 0x45, 0x9f, 0x82, 0xc5, 0xb3, 0x00, 0x95,
//   0x2c, 0x49, 0x10, 0x48, 0x81, 0xff, 0x48}; uint8_t message[16] = {0x33,
//   0x32, 0x34, 0x62, 0x63, 0x39, 0x38, 0x61, 0x37, 0x34, 0x79, 0x00, 0x00,
//   0x00, 0x00, 0x00}; nas_stream_cipher_t stream_cipher = {0}; uint8_t mac[4];
//   stream_cipher.key = key;
//   stream_cipher.key_length = 16;
//   stream_cipher.count = count;
//   stream_cipher.bearer = 0x1f;
//   stream_cipher.direction = 0;
//   stream_cipher.message = message;
//   stream_cipher.blength = 88;
//   // nas_algorithms::nas_stream_encrypt_nia1 (&stream_cipher, mac);
//   // uint32_t mac32 = ntohl(*((uint32_t*) mac));
//   // printf("mac: 0x%x\n", mac32);
// }

// void nia2_test(){
//   uint32_t count = 0x38a6f056;
//   uint8_t key[16] = {0x2b, 0xd6, 0x45, 0x9f, 0x82, 0xc5, 0xb3, 0x00, 0x95,
//   0x2c, 0x49, 0x10, 0x48, 0x81, 0xff, 0x48}; uint8_t message[8] = {0x33,
//   0x32, 0x34, 0x62, 0x63, 0x39, 0x38, 0x40}; nas_stream_cipher_t
//   stream_cipher = {0}; uint8_t mac[4]; stream_cipher.key = key;
//   stream_cipher.key_length = 16;
//   stream_cipher.count = count;
//   stream_cipher.bearer = 0x18;
//   stream_cipher.direction = 0;
//   stream_cipher.message = message;
//   stream_cipher.blength = 58;
//   nas_algorithms::nas_stream_encrypt_nia2 (&stream_cipher, mac);
//   uint32_t mac32 = ntohl(*((uint32_t*) mac));
//   printf("mac: 0x%x\n", mac32);
// }

void buffer_test() {
  uint8_t mac[4] = {0x11, 0x22, 0x33, 0x44};
  uint8_t buffer[6] = {0};
  uint32_t mac32;
  mac32 = ntohl(*((uint32_t *)mac));
  memcpy(buffer, &mac32, 4);
  print_buffer("amf_n1", "uint8_t *mac", mac, 4);
  printf("mac32: 0x%x\n", mac32);
  print_buffer("amf_n1", "buffer", buffer, 4);
  *((uint32_t *)buffer) = htonl(mac32);
  print_buffer("amf_n1", "buffer2", buffer, 4);
}

void xresStar_test() {
  uint8_t opc[16] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                     0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
  uint8_t key[16] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff};
  uint8_t rand[16] = {0x5b, 0x2e, 0x1c, 0x24, 0x28, 0xc7, 0x56, 0x28,
                      0xe4, 0x43, 0xb0, 0xac, 0x89, 0x5d, 0x5b, 0x73};
  uint8_t res[8], ck[16], ik[16], ak[6];
  Authentication_5gaka::f2345(opc, key, rand, res, ck, ik, ak);

  std::string snn = "5G:mnc001.mcc110.3gppnetwork.org";
  OCTET_STRING_t netName;
  OCTET_STRING_fromBuf(&netName, snn.c_str(), snn.length());
  uint8_t S[100];
  S[0] = 0x6B;
  memcpy(&S[1], netName.buf, netName.size);
  printf("snn length(0x%x)\n", netName.size);
  S[1 + netName.size] = (netName.size & 0xff00) >> 8;
  S[2 + netName.size] = (netName.size & 0x00ff);
  for (int i = 0; i < 16; i++) S[3 + netName.size + i] = rand[i];
  S[19 + netName.size] = 0x00;
  S[20 + netName.size] = 0x10;
  for (int i = 0; i < 8; i++) S[21 + netName.size + i] = res[i];
  S[29 + netName.size] = 0x00;
  S[30 + netName.size] = 0x08;

  uint8_t ckik[32];
  memcpy(&key[0], ck, 16);
  memcpy(&key[16], ik, 16);  // KEY

  uint8_t out[32], output[16];
  Authentication_5gaka::kdf(ckik, 32, S, 31 + netName.size, out, 32);
  for (int i = 0; i < 16; i++) output[i] = out[16 + i];
  print_buffer("amf_n1", "XRES*", output, 16);
}

void mac_test() {
  uint8_t opc[16] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                     0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
  uint8_t key[16] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
                     0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff};
  uint8_t rand[16] = {0x6a, 0x89, 0x59, 0xfb, 0x18, 0x8c, 0x73, 0x30,
                      0x8d, 0x67, 0x9f, 0x7b, 0xc8, 0x31, 0x3d, 0x65};
  // uint8_t rand[16] = {0x02, 0x27, 0x40, 0x55, 0xea, 0x5a, 0x2f, 0x7e, 0x3e,
  // 0x87, 0x16, 0x4b, 0xdf, 0x80, 0xff, 0x32}; uint8_t rand[16] = {0x97, 0xce,
  // 0xf6, 0x4e, 0x75, 0xbf, 0x90, 0x7c, 0x01, 0xf0, 0x65, 0x6a, 0xbb, 0xf8,
  // 0x84, 0x02};
  uint8_t res[8], ck[16], ik[16], ak[6];
  Authentication_5gaka::f2345(opc, key, rand, res, ck, ik, ak);
  print_buffer("amf_n1", "XRES = F2(RAND)", res, 8);
  print_buffer("amf_n1", "CK = F3(RAND)", ck, 16);
  print_buffer("amf_n1", "IK = F4(RAND)", ik, 16);
  print_buffer("amf_n1", "IK = F4(RAND)", ak, 16);
  /*
    uint8_t CK[16] = {0xb1, 0xaa, 0xdd, 0xdb, 0x49, 0x84, 0x0f, 0x49, 0x65,
    0x6f, 0xf2, 0x70, 0x31, 0x44, 0x97, 0x86}; uint8_t IK[16] = {0x39, 0x8a,
    0x1b, 0xc9, 0x2e, 0x35, 0xbd, 0x89, 0xb4, 0xe9, 0x5d, 0x03, 0xf4, 0x83,
    0x5b, 0x37}; memcpy(ck, CK, 16); memcpy(ik, IK, 16);
  */
  /******************** XRES to XRES* **************************/
  // std::string snnStr = "5G:mnc002.mcc460.3gppnetwork.org";
  std::string snnStr = "5G:mnc001.mcc110.3gppnetwork.org";
  uint8_t *snn = (uint8_t *)snnStr.c_str();
  int len = snnStr.length();
  uint8_t KEY[32];
  memcpy(&KEY[0], ck, 16);
  memcpy(&KEY[16], ik, 16);
  print_buffer("amf_n1", "XRES* Derivation: KEY", KEY, 32);

  uint8_t S[200];
  S[0] = 0x6B;
  memcpy(&S[1], snn, len);
  S[1 + len] = (len & 0xff00) >> 8;
  S[2 + len] = len & 0x00ff;
  memcpy(&S[3 + len], rand, 16);
  S[19 + len] = 0x00;
  S[20 + len] = 0x10;
  memcpy(&S[21 + len], res, 8);
  S[29 + len] = 0x00;
  S[30 + len] = 0x08;
  print_buffer("amf_n1", "XRES* Derivation: Input", S, 31 + len);

  uint8_t out[32], output[16];
  Authentication_5gaka::kdf(KEY, 32, S, 31 + len, out, 32);
  memcpy(output, out + 16, 16);
  print_buffer("amf_n1", "Result of KDF: XRES*", output, 16);

  /******************* MAC *******************************************/
  // uint8_t sqnak[6] = {0xe4, 0x0f, 0x91, 0xf6, 0x89, 0xf3};
  // uint8_t sqnak[6] = {0x0f, 0xa2, 0x08, 0x8b, 0x0d, 0xfa};
  // uint8_t sqnak[6] = {0xcb, 0x9f, 0x58, 0xa7, 0x8d, 0x98};
  uint8_t sqnak[6] = {0x97, 0x77, 0x9b, 0x30, 0x56, 0x86};
  uint8_t sqn[6];
  for (int i = 0; i < 6; i++) sqn[i] = sqnak[i] ^ ak[i];
  print_buffer("amf_n1", "sqn", sqn, 6);
  uint8_t amf[2] = {0x90, 0x01};
  uint8_t mac_a[8];
  Authentication_5gaka::f1(opc, key, rand, sqn, amf, mac_a);
  print_buffer("amf_n1", "mac_a", mac_a, 8);

  /******************** KAUSF Derivation *****************************/
  memset(S, 0, 200);
  // print_buffer("amf_n1", "reset input S", S, 200);
  S[0] = 0x6A;
  memcpy(&S[1], snn, len);
  S[1 + len] = (len & 0xff00) >> 8;
  S[2 + len] = len & 0x00ff;
  memcpy(&S[3 + len], sqnak, 6);
  S[9 + len] = 0x00;
  S[10 + len] = 0x06;
  print_buffer("amf_n1", "Kausf Derivation: Input", S, 11 + len);

  uint8_t kausf[32];
  Authentication_5gaka::kdf(KEY, 32, S, 11 + len, kausf, 32);
  print_buffer("amf_n1", "Result of KDF: Kausf", kausf, 32);

  /******************* Kseaf Derivation *****************************/
  memset(S, 0, 200);
  // std::string newsnnStr = "5G:mnc002.mcc460.3gppnetwork.org";
  std::string newsnnStr = "5G:mnc001.mcc110.3gppnetwork.org";
  uint8_t *newsnn = (uint8_t *)newsnnStr.c_str();
  len = newsnnStr.length();
  S[0] = 0x6C;
  memcpy(&S[1], newsnn, len);
  S[1 + len] = (len & 0xff00) >> 8;
  S[2 + len] = len & 0x00ff;
  print_buffer("amf_n1", "Kseaf Derivation: Input", S, 3 + len);

  uint8_t kseaf[32];
  Authentication_5gaka::kdf(kausf, 32, S, 3 + len, kseaf, 32);
  print_buffer("amf_n1", "Result of KDF: Kseaf", kseaf, 32);

  /******************* Kamf Derivation ******************************/
  uint8_t abba[2] = {0x00, 0x00};
  // std::string supiStr = "";
  // std::string supiStr = "imsi-460110123456789";
  // std::string supiStr = "460020100001103";
  std::string supiStr = "110010123456789";
  uint8_t *supi = (uint8_t *)supiStr.c_str();
  len = supiStr.length();
  // uint8_t supi[8] = {0x46, 0x0F, 0x11, 0x01, 0x23, 0x45, 0x67, 0x89};
  // len = 8;
  memset(S, 0, 200);
  S[0] = 0x6D;
  memcpy(&S[1], supi, len);
  S[1 + len] = (len & 0xff00) >> 8;
  S[2 + len] = len & 0x00ff;
  memcpy(&S[3 + len], abba, 2);
  S[5 + len] = 0x00;
  S[6 + len] = 0x02;
  print_buffer("amf_n1", "Kamf Derivation: Input", S, 7 + len);

  uint8_t kamf[32];
  Authentication_5gaka::kdf(kseaf, 32, S, 7 + len, kamf, 32);
  print_buffer("amf_n1", "Result of KDF: Kamf", kamf, 32);

  // /******************** Knas_int with NIA1 Derivation ****************/
  //   memset(S, 0, 200);
  //   S[0] = 0x69;
  //   S[1] = 0x02;
  //   S[2] = 0x00;
  //   S[3] = 0x01;
  //   S[4] = 0x01;
  //   S[5] = 0x00;
  //   S[6] = 0x01;
  //   print_buffer("amf_n1", "Kamf_int with NIA1 Derivation: Input", S, 7);

  //   uint8_t kamf_int[16];
  //   memset(out, 0, 32);
  //   Authentication_5gaka::kdf(kamf, 32, S, 7, out, 32);
  //   memcpy(kamf_int, out+16, 16);
  //   print_buffer("amf_n1" ,"Result of KDF: Kamf_int", kamf_int, 16);

  // /********************** NIA1 Algrithms ******************************/
  //   uint32_t count = 0;
  //   uint8_t message[13] = {0x00 ,0x7e, 0x00, 0x5d, 0x01, 0x01, 0x02, 0xf0,
  //   0xf0, 0xe1, 0x36, 0x01, 0x02}; nas_stream_cipher_t stream_cipher = {0};
  //   uint8_t mac[4];
  //   stream_cipher.key = kamf_int;
  //   stream_cipher.key_length = 16;
  //   stream_cipher.count = count;
  //   stream_cipher.bearer = 0x01;
  //   stream_cipher.direction = 1;
  //   stream_cipher.message = message;
  //   stream_cipher.blength = 13*8;
  //   nas_algorithms::nas_stream_encrypt_nia1 (&stream_cipher, mac);
  //   //uint32_t mac32 = ntohl(*((uint32_t*) mac));
  //   print_buffer("amf_n1", "calculated MAC", mac, 4);
}

void new_mac_test() {
  uint8_t Kausf_Input[43] = {
      0x6a, 0x35, 0x47, 0x3a, 0x6d, 0x6e, 0x63, 0x30, 0x30, 0x32, 0x2e,
      0x6d, 0x63, 0x63, 0x34, 0x36, 0x30, 0x2e, 0x33, 0x67, 0x70, 0x70,
      0x6e, 0x65, 0x74, 0x77, 0x6f, 0x72, 0x6b, 0x2e, 0x6f, 0x72, 0x67,
      0x00, 0x20, 0x2b, 0x9f, 0x0a, 0xf9, 0x8d, 0x89, 0x00, 0x06};
  uint8_t key[32] = {0xc4, 0xda, 0x59, 0x6a, 0xd1, 0x36, 0xe7, 0xe7,
                     0x45, 0x32, 0x40, 0x82, 0xdd, 0xa3, 0x30, 0x98,
                     0x58, 0x2b, 0x9a, 0x60, 0x5d, 0x73, 0x25, 0xf4,
                     0x3c, 0x2b, 0x8d, 0x5a, 0x98, 0x57, 0xc7, 0xa5};
  uint8_t Kausf[32];
  Authentication_5gaka::kdf(key, 32, Kausf_Input, 43, Kausf, 32);
  print_buffer("amf_n1", "Kausf out", Kausf, 32);

  uint8_t Kseaf[32];
  uint8_t Kseaf_Input[35] = {
      0x6c, 0x35, 0x47, 0x3a, 0x6d, 0x6e, 0x63, 0x30, 0x30, 0x32, 0x2e, 0x6d,
      0x63, 0x63, 0x34, 0x36, 0x30, 0x2e, 0x33, 0x67, 0x70, 0x70, 0x6e, 0x65,
      0x74, 0x77, 0x6f, 0x72, 0x6b, 0x2e, 0x6f, 0x72, 0x67, 0x00, 0x20};
  Authentication_5gaka::kdf(Kausf, 32, Kseaf_Input, 35, Kseaf, 32);
  print_buffer("amf_n1", "Kseaf out", Kseaf, 32);

  uint8_t Kamf[32];
  uint8_t Kamf_Input[22] = {0x6d, 0x34, 0x36, 0x30, 0x30, 0x32, 0x30, 0x31,
                            0x30, 0x30, 0x30, 0x30, 0x31, 0x31, 0x30, 0x33,
                            0x00, 0x0f, 0x00, 0x00, 0x00, 0x02};
  Authentication_5gaka::kdf(Kseaf, 32, Kamf_Input, 22, Kamf, 32);
  print_buffer("amf_n1", "Kseaf out", Kamf, 32);

  // uint8_t Knas_int[16], out[32];
  // uint8_t Knas_int_Input[7] = {0x69, 0x02, 0x00, 0x01, 0x02, 0x00, 0x01};
  // Authentication_5gaka::kdf(Kamf, 32, Knas_int_Input, 7, out, 32);
  // print_buffer("amf_n1", "Knas out", out, 32);
  // memcpy(Knas_int, out+16, 16);
  // print_buffer("amf_n1", "Knas_int out", Knas_int, 16);
}

void istream_test() { char c = '1'; }

#endif