#include <Arduino.h>
#include <avr/io.h>
#include <stdlib.h>

// Define constants and function prototypes
#define fe25519_add avrnacl_fe25519_add
#define fe25519_sub avrnacl_fe25519_sub
#define fe25519_red avrnacl_fe25519_red

extern "C"
{
  void fe25519_sub(unsigned char* r, const unsigned char* x, const unsigned char* y);
  void fe25519_add(unsigned char* r, const unsigned char* x, const unsigned char* y);
  void fe25519_red(unsigned char* r, unsigned char* C);
  char bigint_subp(unsigned char* r, const unsigned char* a);
  char bigint_square256(unsigned char* r, const unsigned char* a);
  char bigint_mul256(unsigned char* r, const unsigned char* a, const unsigned char* b);
  void bigint_mul121666(unsigned char* r, const unsigned char* x);
}

void fe25519_freeze(unsigned char* r);
void fe25519_cmov(unsigned char* r, const unsigned char* x, unsigned char b);
void fe25519_mul(unsigned char* r, const unsigned char* x, const unsigned char* y);
void fe25519_square(unsigned char* r, const unsigned char* x);
void fe25519_invert(unsigned char* r, const unsigned char* x);
void work_cswap(unsigned char* x0, unsigned char* z0, unsigned char* x1, unsigned char* z1, char b);
void mladder(unsigned char* x_G, unsigned char* zr, const unsigned char s[32]);
//void fe25519_mul121666(unsigned char* r, const unsigned char* x);

static const unsigned char _121666[32] = {0x42, 0xDB, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
// x_G represent the x-coordinate of the base point (or the generator).
unsigned char x_G[32] = {0x09};

int main()
{
  Serial.begin(500000);
  // scalar represent the scalar multiplcation value.
  //unsigned char scalar[32] = {0x05};
  //unsigned char scalar[32] = {0x64};

  //scalar = 10 000  
  //unsigned char scalar[32] = {0x10, 0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  
  //scalar = 100 000
  //unsigned char scalar[32] = {0xA0, 0x86, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

  //scalar = 1 000 000
  unsigned char scalar[32] = {0x40, 0x42, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  
  //scalar = 115352209786349161865613911055064319759612420267277587660521006566223356815243
  //unsigned char scalar[32] = {0x8B,	0xE7,	0xB6,	0x94,	0xF3,	0x23,	0x8D,	0xB7,	0xA3,	0x2F,	0x8A,	0x27,	0x2B,	0x5D,	0x15,	0x01,	0xFB,	0xBC,	0x3F,	0x4D,	0xF4,	0xC6,	0x1B,	0xA0,	0xEB,	0xF9,	0x5F,	0x59,	0x7C,	0x09,	0x07,	0xFF};
  
  //scalar = 15021032309091536700857323171072105375962898139453978062622797955632404785066
  //unsigned char scalar[32] = {0xAA,	0x67,	0xBA,	0x79,	0x54,	0x31,	0xBF,	0x93,	0xE7,	0x2A,	0xD3,	0x1E,	0x1E,	0xF1,	0x2D,	0x60,	0x75,	0x4E,	0xCC,	0x64,	0xD4,	0x08,	0x97,	0xEF,	0xA0,	0xB5,	0x39,	0x4E,	0x5E,	0x9A,	0x35,	0x21};

  //scalar = 9917086214443545137717076255545409398971448020892002913844315200310097123296
  //unsigned char scalar[32] = {0xE0,	0x97,	0xBE,	0x8C,	0x8E,	0xEE,	0x30,	0x64,	0x48,	0xB1,	0xCD,	0x2E,	0x05,	0x2E,	0x1B,	0x26,	0xEC,	0x98,	0xD8,	0xD2,	0xD0,	0x10,	0xC6,	0x90,	0x99,	0x0D,	0xCA,	0xD5,	0x33,	0xDF,	0xEC,	0x15};
  
  //scalar = 33078161054944423906682172360831403590213522247660334228980404087165772315686
  //unsigned char scalar[32] = {0x26,	0x3C,	0x77,	0xE5,	0xDF,	0xF0,	0x8C,	0xCC,	0x1D,	0x29,	0x88,	0xCF,	0x5C,	0x60,	0xD3,	0xAB,	0x1D,	0xA6,	0xE9,	0xDA,	0x05,	0x0E,	0xD0,	0x31,	0x0B,	0xD1,	0x60,	0x62,	0x60,	0x93,	0x21,	0x49};

  //scalar = 5099684974583650596756194485002365559400503598311394943634172142812572199125
  //unsigned char scalar[32] = {0xD5,	0xA8,	0x08,	0x99,	0xD4,	0xB7,	0xC4,	0xB3,	0xF1,	0x69,	0xC2,	0x8E,	0x4F,	0x46,	0x37,	0x5E,	0x95,	0xF8,	0xE5,	0x16,	0x0D,	0x0A,	0x3D,	0x8E,	0x94,	0x40,	0x31,	0xBE,	0xC4,	0x51,	0x46,	0x0B};
  
  //scalar = 57896044618658097711785492504343953926634992332820282019728792003956564819967
  //unsigned char scalar[32] = {0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0xFF,	0x7F};

  //scalar = 57862248164269782305289437235705246244466051046508798961432221078586627364832
  //unsigned char scalar[32] = {0xE0,	0x97,	0xBE,	0x8C,	0x8E,	0xEE,	0x30,	0x64,	0x48,	0xB1,	0xCD,	0x2E,	0x05,	0x2E,	0x1B,	0x26,	0xEC,	0x98,	0xD8,	0xD2,	0xD0,	0x10,	0xC6,	0x90,	0x99,	0x0D,	0xCA,	0xD5,	0x33,	0xDF,	0xEC,	0x7F};

  //scalar[32] = 29879169281873086039215122262119148411048782014056941977100548395656638820678
  //unsigned char scalar[32] = {0x46,	0xF1,	0x02,	0xC7,	0xEC,	0x29,	0xA6,	0x73,	0xDA,	0xB0,	0xDA,	0x17,	0xB5,	0x34,	0x52,	0x25,	0xD2,	0x8F,	0xA0,	0xBC,	0x07,	0xC5,	0xC7,	0x7A,	0x3E,	0x11,	0x0D,	0xC7,	0xB0,	0x02,	0x0F,	0x42};
  
  //scalar[32] = 53368152853790257344399214764518887256994347548617861699334612342467252058980
  //unsigned char scalar[32] = {0x64,	0xFB,	0xC9,	0xB1,	0x06,	0x2A,	0x72,	0xCE,	0xA2,	0x37,	0xED,	0x28,	0xCE,	0xD8,	0x8E,	0x93,	0x4F,	0xA4,	0x0C,	0xF2,	0x5B,	0x15,	0x62,	0x3A,	0x1B,	0xC2,	0x3A,	0x29,	0xD8,	0x4D,	0xFD,	0x75};

  //scalar[32] = 14421227213452706288997547019498743081723671628402337036933242897380268921048
  //unsigned char scalar[32] = {0xD8,	0x50,	0xC1,	0x02,	0x76,	0x39,	0x2D,	0x4A,	0xB1,	0x65,	0x98,	0xC8,	0xAD,	0x47,	0xCB,	0xE5,	0xC6,	0xAD,	0xBF,	0xEE,	0x6B,	0xA9,	0x0E,	0x34,	0x23,	0xC1,	0x55,	0xCE,	0x16,	0x20,	0xE2,	0x1F};

  //scalar[32] = 25363787918498526884928428731816802260991346960921699205993613487067137215710
  //unsigned char scalar[32] = {0xDE,	0x8C,	0x06,	0x8B,	0x65,	0x64,	0x5E,	0x10,	0x4C,	0xD7,	0xD3,	0x09,	0x49,	0x37,	0xDD,	0xB1,	0xC4,	0x8B,	0x6F,	0xBE,	0x5F,	0x57,	0xC3,	0xF7,	0xEE,	0xC9,	0x2F,	0x85,	0x2D,	0x65,	0x13,	0x38};

  //scalar[32] = 35555175223872809342946438537436939354076616378314247556439800725119816103857
  //unsigned char scalar[32] = {0xB1,	0xF7,	0x5A,	0x00,	0xC2,	0x78,	0x38,	0x80,	0x8F,	0x02,	0xF1,	0xA2,	0xA9,	0x60,	0x6E,	0x10,	0x5F,	0x3E,	0xA8,	0x50,	0x56,	0x0B,	0xCB,	0x90,	0x31,	0x03,	0xA5,	0x4A,	0x1A,	0x84,	0x9B,	0x4E};

  //scalar[32] = 14874757641263238545022211312407986987125093450501594436022931667277167702133
  //unsigned char scalar[32] = {0x75,	0xB0,	0x87,	0x75,	0x2B,	0x0C,	0x3A,	0x5C,	0x24,	0x98,	0xEC,	0xEB,	0x60,	0xC9,	0x35,	0x91,	0xC6,	0x09,	0x10,	0xFC,	0xFB,	0x5A,	0xF4,	0xC1,	0x52,	0x91,	0x5F,	0x54,	0x81,	0xD0,	0xE2,	0x20};
  
  //scalar[32] = 26884029249486940213838632402966051184492120841600662922574746171831817099863
  //unsigned char scalar[32] = {0x57,	0x5A,	0x54,	0x55,	0x03,	0x3C,	0x55,	0x32,	0x88,	0x30,	0x4B,	0xD0,	0xB8,	0xD5,	0x45,	0x96,	0xDB,	0xD7,	0x75,	0xB8,	0x6F,	0x80,	0x14,	0x76,	0x38,	0xBF,	0x7D,	0x79,	0x42,	0xD2,	0x6F,	0x3B};

  //scalar[32] = 29991404550501958545067133878419532572517322921937988079569300225193733290399
  //unsigned char scalar[32] = {0x9F,	0x79,	0xBC,	0x9D,	0x42,	0x3A,	0xD7,	0x93,	0x2A,	0xBF,	0x80,	0x70,	0xD5,	0x3E,	0x6F,	0x34,	0xAB,	0x55,	0x09,	0x6D,	0xBE,	0xB5,	0x63,	0x2E,	0x89,	0xB1,	0xC7,	0x43,	0x8E,	0x88,	0x4E,	0x42};

  //scalar[32] = 19991909401879732796567999228478050578477870226966056432665435166152094364413
  //unsigned char scalar[32] = {0xFD,	0xB2,	0x9F,	0xF2,	0x4B,	0xF8,	0x55,	0x65,	0x3E,	0x97,	0xD0,	0xDB,	0xEB,	0xE4,	0x54,	0x4D,	0x2C,	0x22,	0xF7,	0x03,	0x5A,	0xC5,	0x7D,	0x8C,	0xAB,	0x29,	0xA4,	0x2C,	0x0D,	0x05,	0x33,	0x2C};
  
  //scalar[32] = 19059928428662903247433881844931121283297291675510572238734505925995633272776
  //unsigned char scalar[32] = {0xC8,	0x67,	0xD3,	0xC9,	0x70,	0xC4,	0xFA,	0x2A,	0x1D,	0x98,	0x7C,	0x1F,	0xF8,	0xD3,	0xFA,	0xEC,	0x85,	0xB4,	0x1A,	0x77,	0xBD,	0xE9,	0xF2,	0xF6,	0x16,	0xA3,	0xB3,	0x97,	0x85,	0x89,	0x23,	0x2A};

  //scalar[32] = 14136337582717731973248418904357649225837654591239254543772784446456732932809
  //unsigned char scalar[32] = {0xC9,	0x5A,	0xD1,	0x83,	0x25,	0x90,	0x64,	0xA7,	0x84,	0x4F,	0x36,	0x93,	0xE9,	0x89,	0x91,	0x88,	0x20,	0x99,	0xC7,	0xC2,	0xC9,	0xE0,	0xEA,	0x73,	0xB6,	0x64,	0xA0,	0x19,	0x2F,	0xE2,	0x40,	0x1F};
  
  //scalar[32] = 25178807741889209515255970623677205909616953022209124245819749911486045661172
  //unsigned char scalar[32] = {0xF4,	0xA7,	0xC8,	0x72,	0xD3,	0xED,	0x66,	0xCE,	0x7F,	0x82,	0xDD,	0x66,	0xB4,	0xDB,	0xBC,	0xB8,	0xA5,	0x3F,	0xB9,	0xC1,	0x84,	0xF6,	0xBC,	0x85,	0x83,	0xFE,	0x91,	0x6E,	0x3D,	0xB3,	0xAA,	0x37};
  
  //scalar[32] = 47903968643362206853062453869023907318321511015031494188824290223390080439812
  //unsigned char scalar[32] = {0x04,	0x16,	0xA1,	0x84,	0xE1,	0x79,	0x04,	0x0D,	0x5F,	0x77,	0x31,	0xCC,	0x2C,	0x36,	0xF1,	0x55,	0xC9,	0xFA,	0x3C,	0x65,	0xEA,	0x22,	0x27,	0xA0,	0x52,	0x88,	0xCC,	0x57,	0x77,	0xAF,	0xE8,	0x69};

  //scalar[32] = 10537672528690362294424921837476431657603602741106616271064188083955875491943
  //unsigned char scalar[32] = {0x67,	0xB0,	0x48,	0xBE,	0x7F,	0xCD,	0xF7,	0x53,	0x0B,	0xFD,	0xFC,	0xE0,	0xED,	0x4C,	0x92,	0x92,	0xD8,	0x6B,	0xC3,	0x2B,	0x96,	0xAD,	0x7C,	0x97,	0x77,	0xB1,	0xCF,	0x92,	0x7D,	0x1C,	0x4C,	0x17};

  //scalar[32] = 7635961459983438163291947046655157781036170275317185877449078370472998181746
  //unsigned char scalar[32] = {0x72,	0x9B,	0x73,	0x4D,	0xA7,	0x8E,	0x6B,	0x56,	0xB4,	0xB0,	0x8D,	0x33,	0x63,	0xFD,	0x1F,	0xAC,	0xA7,	0xBB,	0xC7,	0x69,	0xC5,	0x5C,	0x78,	0x75,	0x79,	0x9C,	0x48,	0x23,	0x10,	0xCD,	0xE1,	0x10};

  //scalar[32] = 57895515540375282676755747512113451878792272201101534522822518141245036137294
  //unsigned char scalar[32] = {0x4E,	0x8B,	0x33,	0xA1,	0xCC,	0x70,	0x39,	0xAD,	0xD6,	0x5C,	0xFA,	0x2C,	0xD7,	0x1B,	0xE8,	0xF4,	0xD2,	0xE2,	0xF5,	0x10,	0xBA,	0xEC,	0xE5,	0xF9,	0xBF,	0x9D,	0x52,	0x65,	0x57,	0xB3,	0xFF,	0x7F};

  //scalar[32] = 22275689968883995129123745056470251759723125483980435536993144601351865279135
  //unsigned char scalar[32] = {0x9F,	0xA2,	0xDC,	0xB0,	0xC9,	0xB0,	0xDB,	0x45,	0xCF,	0x7E,	0x67,	0x8B,	0x30,	0xBC,	0x5A,	0x5D,	0xBD,	0xCC,	0x85,	0x8E,	0x9C,	0x15,	0x44,	0xA2,	0x33,	0x78,	0x14,	0x6F,	0xFE,	0x97,	0x3F,	0x31};

  //scalar[32] = 50578967930504594777951334725988871628071732666203728050169031231517763165026
  //unsigned char scalar[32] = {0x62,	0xDB,	0x55,	0xAE,	0xA5,	0xC5,	0xB9,	0x3E,	0x7D,	0x41,	0x74,	0x5C,	0xF6,	0x75,	0x1F,	0x3C,	0x95,	0xF2,	0x91,	0x5A,	0x3D,	0xBD,	0x14,	0x20,	0xA4,	0xFA,	0xB7,	0x6E,	0x6D,	0xAE,	0xD2,	0x6F};

  unsigned char z[32]; // Result of mladder

  //scalar[0] &= 248;
  //scalar[31] &= 127;
  //scalar[31] |= 64;

  Serial.println(" ");
  Serial.println(" ");
  Serial.println(" ============================================================================== ");
  Serial.println(" ");
  printArray(" ==> Initialization value of x_G before mladder", x_G, 32);
  printArray(" ==> Initialization value of z before mladder", z, 32);
  printArray(" ==> Initialization value of scalar before mladder", scalar, 32);
  Serial.println(" ============================================================================== ");

  mladder(x_G, z, scalar);
  Serial.println(" ============================================================================== ");
  printArray(" After mladder function, we have to calculate the multiplicative inverse of the z0 = ", z, 32);
  Serial.println(" ============================================================================== ");
  fe25519_invert(z, z);
  printArray(" The multiplicative inverse of the z0 ", z, 32);
  fe25519_mul(x_G, x_G, z);

  fe25519_freeze(x_G);

  Serial.print("Result: ");
  for (int i = 0; i < 32; i++)
  {
    Serial.print(x_G[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}

// Implement mladder function here
void mladder(unsigned char* x_G, unsigned char* zr, const unsigned char s[32])
{
  unsigned char x0[32] = {0x01};
  unsigned char z0[32] = {0x00};
  unsigned char x1[32] = {0x09};
  unsigned char z1[32] = {0x01};
  unsigned char bit, swap = 0;
  signed char j = 6;
  //signed char i;

  // Print the scalar values in binary starting from scalar[31] to scalar[0]
  Serial.println("Printing scalar in binary:");
  for (signed char i = 31; i >= 0; i--) {
    Serial.print("scalar[");
    Serial.print(i);
    Serial.print("] = ");
    for (j = 7; j >= 0; j--)
    {
      // Loop over each bit in the byte
      Serial.print((s[i] >> j) & 1);  // Extract and print each bit
    }
    Serial.println();  // Move to the next line after each byte
  }
  Serial.println("==========================");

  printArray("x0 inside mladder before for loop", x0, 32);
  printArray("z0 inside mladder before for loop", z0, 32);
  printArray("x1 inside mladder before for loop", x1, 32);
  printArray("z1 inside mladder before for loop", z1, 32);
  j = 6;
  //i = 31;
  for (signed char i = 31; i >= 0; i--)
  {
    while (j >= 0)
    {
      Serial.print(" ===============> Inside the mladder function, in the i = ");
      Serial.print(i);
      Serial.print(", j = ");
      Serial.println(j);

      bit = 1 & (s[i] >> j);

      Serial.print("Current bit = bit = 1 & (s[");
      Serial.print(i);
      Serial.print("] >> ");
      Serial.print(j);
      Serial.print(") = ");
      Serial.println(bit, BIN);

      Serial.print("Previous swap = ");
      Serial.println(swap, BIN);
      Serial.println(" ---------------------------- ");

      swap ^= bit;

      Serial.print("Current swap = previous swap XOR current bit = ");
      Serial.println(swap, BIN);
      Serial.println(" ---------------------------- ");

      // Before work_cswap
      printArray("x0 before work_cswap", x0, 32);
      printArray("z0 before work_cswap", z0, 32);
      printArray("x1 before work_cswap", x1, 32);
      printArray("z1 before work_cswap", z1, 32);
      //Serial.print("swap value: ");
      //Serial.println(swap, BIN); // Print in binary format
      Serial.println(" ---------------------------- ");

      work_cswap(x0, z0, x1, z1, swap);

      // After work_cswap
      printArray("x0 after work_cswap", x0, 32);
      printArray("z0 after work_cswap", z0, 32);
      printArray("x1 after work_cswap", x1, 32);
      printArray("z1 after work_cswap", z1, 32);

      swap = bit;

      Serial.println(" ---------------------------- Calling ladderstep Function ---------------------------- ---------------------------- ");
      ladderstep(x0, z0, x1, z1);
      Serial.println(" ---------------------------- End of -> ladderstep Function ---------------------------- ---------------------------- ");
      // After ladderstep
      printArray("x0 after ladderstep", x0, 32);
      printArray("z0 after ladderstep", z0, 32);
      printArray("x1 after ladderstep", x1, 32);
      printArray("z1 after ladderstep", z1, 32);
      
      Serial.print(" ===============> The End of the iteration i = ");
      Serial.print(i);
      Serial.print(", j = ");
      Serial.println(j);
      
      Serial.println(" ");

      j -= 1;
    }
    j = 7;
  }
  Serial.println(" ===============> At end of the mladder function: ");
  Serial.print("swap value: ");
  Serial.println(swap, BIN); // Print in binary format
  work_cswap(x0, z0, x1, z1, swap);
  // After ladderstep
  printArray("x0 after last swap", x0, 32);
  printArray("z0 after last swap", z0, 32);
  printArray("x1 after last swap", x1, 32);
  printArray("z1 after last swap", z1, 32);

  memcpy(x_G, x0, 32);
  memcpy(zr, z0, 32);
}

// Implement work_cswap and ladderstep functions here
void work_cswap(unsigned char* x0, unsigned char* z0, unsigned char* x1, unsigned char* z1, char b)
{
  unsigned char t[32];

  fe25519_cmov(t, x0, b);
  fe25519_cmov(x0, x1, b);
  fe25519_cmov(x1, t, b);
  
  fe25519_cmov(t, z0, b);
  fe25519_cmov(z0, z1, b);
  fe25519_cmov(z1, t, b);
}

void fe25519_cmov(unsigned char* r, const unsigned char* x, unsigned char b)
{
  unsigned long mask = b;
  mask = -mask;
  for (unsigned char i = 0; i < 32; i++)
  {
    r[i] ^= mask & (x[i] ^ r[i]);
  }
}

/* reduction modulo 2^255-19 */
void fe25519_freeze(unsigned char* r)
{
  unsigned char rt[32] = {0x00};
  unsigned char c ;
  c = bigint_subp(rt, r);
  fe25519_cmov(r, rt, 1 - c);
  /*c = bigint_subp(rt, r);
  fe25519_cmov(r, rt, 1 - c);*/
}

void fe25519_mul(unsigned char* r, const unsigned char* x, const unsigned char* y)
{
  unsigned char t[64];
  cli();  // Disable interrupts
  bigint_mul256(t, x, y);
  sei();  // Enable interrupts
  fe25519_red(r, t);
  //fe25519_freeze(r);
}

void fe25519_square(unsigned char* r, const unsigned char* x)
{
  unsigned char t[64];
  cli();  // Disable interrupts
  bigint_square256(t, x);
  sei();  // Enable interrupts
  fe25519_red(r, t);
  //fe25519_freeze(r);
}

void ladderstep(unsigned char* x0, unsigned char* z0, unsigned char* x1, unsigned char* z1)
{
  Serial.println(" ##################### Start of ladderstep ------> ");
  unsigned char t1[32] = {0x00};
  unsigned char t2[32] = {0x00};

  printArray("1. x1 = ", x1, 32);
  printArray("1. z1 = ", z1, 32);

  fe25519_add(t1, x1, z1);
  
  printArray("1. fe25519_add(t1, x1, z1) =============================> C = t1 = x1 + z1 = ", t1, 32);
  /*fe25519_freeze(t1);  // Apply freeze if needed
  printArray("1.  t1 after freeze = ", t1, 32);
  Serial.println(" ");*/
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("2. x1 = ", x1, 32);
  printArray("2. z1 = ", z1, 32);
  
  fe25519_sub(x1, x1, z1);
  
  printArray("2. fe25519_sub(x1, x1, z1)=============================> D = x1 = x1 - z1 = ", x1, 32);
  /*fe25519_freeze(x1);  // Apply freeze if needed
  printArray("2.  x1 after freeze = ", x1, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("3. x0 = ", x0, 32);
  printArray("3. z0 = ", z0, 32);

  fe25519_add(z1, x0, z0);
  
  printArray("3. fe25519_add(z1, x0, z0)=============================> A = z1 = x0 + z0 = ", z1, 32);
  /*fe25519_freeze(z1);  // Apply freeze if needed
  printArray("3.  z1 after freeze = ", z1, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("4. x0 = ", x0, 32);
  printArray("4. z0 = ", z0, 32);
  
  fe25519_sub(x0, x0, z0);
  
  printArray("4. fe25519_sub(x0, x0, z0)=============================> B = x0 = x0 - z0 = ", x0, 32);
  // I think that we have to add (fe25519_freeze) function here
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("5. x0 = ", x0, 32);
  printArray("5. t1 = ", t1, 32);
  
  fe25519_mul(t1, t1, x0);
  
  printArray("5. fe25519_mul(t1, t1, x0)=============================> CB = t1 = t1 * x0 = ", t1, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("6. x1 = ", x1, 32);
  printArray("6. z1 = ", z1, 32);
  
  fe25519_mul(x1, x1, z1);
  
  printArray("6. fe25519_mul(x1, x1, z1)=============================> AD = x1 = x1 * z1 = ", x1, 32);
  /*fe25519_freeze(x1);  // we have to study if it is nessary to add this (fe25519_freeze) function or not, because there is already one or two exist in the (fe25519_mul) function
  printArray("6.  x1 after freeze = ", x1, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("7. z1 = ", z1, 32);
  
  fe25519_square(z1, z1);
  
  printArray("7. fe25519_square(z1, z1)=============================> AA = z1 = z1 ^ 2 = ", z1, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("8. x0 = ", x0, 32);
  
  fe25519_square(x0, x0);
  
  printArray("8. fe25519_square(x0, x0)=============================> BB = x0 = x0 ^ 2 = ", x0, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("9. z1 = ", z1, 32);
  printArray("9. x0 = ", x0, 32);
  
  fe25519_sub(t2, z1, x0);
  
  printArray("9. fe25519_sub(t2, z1, x0)=============================> AA - BB = t2 = z1 * x0 = ", t2, 32);
  // I think that we have to add (fe25519_freeze) function here
  // I think that there is no need to the reducation function, and we have to study this in the future
  /*fe25519_red(t2, t2);
  printArray("9. t2 after reducation = ", t2, 32);*/
  /*fe25519_freeze(t2);  // Apply freeze if needed
  printArray("9. t2 after freeze = ", t2, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("10. t2 = ", t2, 32);
  printArray("10. _121666 = ", _121666, 32);
  //fe25519_mul121666(z0, t2);
  
  bigint_mul121666(z0,t2);
  
  printArray("10. bigint_mul121666(z0, t2)=============================> FourXZ * 121666 = A24E = z0 = t2 * _121666 = ", z0, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("11. z0 = ", z0, 32);
  printArray("11. x0 = ", x0, 32);
  
  fe25519_add(z0, z0, x0);
  
  printArray("11. fe25519_add(z0, z0, x0)=============================> A24E + BB = z0 = z0 * x0 = ", z0, 32);
  /*fe25519_freeze(z0);  // Apply freeze if needed
  printArray("11. z0 after freeze = ", z0, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("12. z0 = ", z0, 32);
  printArray("12. t2 = ", t2, 32);
  
  fe25519_mul(z0, z0, t2);
  
  printArray("12. fe25519_mul(z0, z0, t2)=============================> Z0 Final Value = FourXZ * (A24E + BB) = z0 * t2 = ", z0, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("13. z1 = ", z1, 32);
  printArray("13. x0 = ", x0, 32);
  
  fe25519_mul(x0, z1, x0);
  
  printArray("13. fe25519_mul(x0, z1, x0)=============================> X0 Final Value = AA * BB = x0 = z1 * x0 = ", x0, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("14. x1 = ", x1, 32);
  printArray("14. t1 = ", t1, 32);
  
  fe25519_sub(z1, x1, t1);
  
  printArray("14. fe25519_sub(z1, x1, t1)=============================> CB - AD = z1 = x1 * t1 = ", z1, 32);
  /*fe25519_freeze(z1);  // Apply freeze if needed
  printArray("14. z1 after freeze = ", z1, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("15. z1 = ", z1, 32);
  
  fe25519_square(z1, z1);
  
  printArray("15. fe25519_square(z1, z1)=============================> z1 = z1 ^2 = ", z1, 32);
  /*fe25519_freeze(z1);  // Apply freeze if needed
  printArray("15. z1 after freeze = ", z1, 32);*/
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("16. z1 = ", z1, 32);
  printArray("16. x_G = ", x_G, 32);
  
  fe25519_mul(z1, z1, x_G); // Multiply by x_G instead of x0
  
  printArray("16. fe25519_mul(z1, z1, x_G)=============================> Z1 Final Value = z1 * x_G = ", z1, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("17. t1 = ", t1, 32);
  printArray("17. x1 = ", x1, 32);
  
  fe25519_add(x1, t1, x1);
  
  // I think that we have to add (freeze) function here
  printArray("17. fe25519_add(x1, t1, x1)=============================> CB + AD = x1 = t1 + x1 = ", x1, 32);
  Serial.println(" ");
  // ---------------------------------------------------------------------------------------------------------------------------------------
  printArray("18. x1 = ", x1, 32);
  
  fe25519_square(x1, x1);
  
  printArray("18. fe25519_square(x1, x1)=============================> X1 Final Value = x1 ^ 2 = ", x1, 32);
  /*fe25519_freeze(x1);  // Apply freeze if needed
  printArray("18. x1 after freeze = ", x1, 32);*/
  Serial.println(" ");
}
/*//bigint_mul256(t, x, _121666);
void fe25519_mul121666(unsigned char* r, const unsigned char* x)
{
  unsigned char t[64];
  cli();  // Disable interrupts
  bigint_mul121666(t,x);
  sei();  // Enable interrupts
  fe25519_red(r, t);
}*/

void fe25519_invert(unsigned char* r, const unsigned char* x)
{
  unsigned char z2[32], z11[32], z2_10_0[32], z2_50_0[32], z2_100_0[32], t0[32], t1[32];
  unsigned char i;

	/* 2 */ fe25519_square(z2, x);
	/* 4 */ fe25519_square(t1, z2);
	/* 8 */ fe25519_square(t0, t1);
	/* 9 */ fe25519_mul(z2_10_0, t0, x);
	/* 11 */ fe25519_mul(z11, z2_10_0, z2);
	/* 22 */ fe25519_square(t0, z11);
	/* 2^5 - 2^0 = 31 */ fe25519_mul(z2_10_0, t0, z2_10_0);

	/* 2^6 - 2^1 */ fe25519_square(t0,z2_10_0);
	/* 2^7 - 2^2 */ fe25519_square(t1,t0);
	/* 2^8 - 2^3 */ fe25519_square(t0,t1);
	/* 2^9 - 2^4 */ fe25519_square(t1,t0);
	/* 2^10 - 2^5 */ fe25519_square(t0,t1);
	/* 2^10 - 2^0 */ fe25519_mul(z2_10_0,t0,z2_10_0);

	/* 2^11 - 2^1 */ fe25519_square(t0,z2_10_0);
	/* 2^12 - 2^2 */ fe25519_square(t1,t0);
	/* 2^20 - 2^10 */ for (i = 2;i < 10;i += 2){ fe25519_square(t0,t1); fe25519_square(t1,t0); }
	/* 2^20 - 2^0 */ fe25519_mul(z2_50_0,t1,z2_10_0);

	/* 2^21 - 2^1 */ fe25519_square(t0,z2_50_0);
	/* 2^22 - 2^2 */ fe25519_square(t1,t0);
	/* 2^40 - 2^20 */ for (i = 2;i < 20;i += 2) { fe25519_square(t0,t1); fe25519_square(t1,t0); }
	/* 2^40 - 2^0 */ fe25519_mul(t0,t1,z2_50_0);

	/* 2^41 - 2^1 */ fe25519_square(t1,t0);
	/* 2^42 - 2^2 */ fe25519_square(t0,t1);
	/* 2^50 - 2^10 */ for (i = 2;i < 10;i += 2) { fe25519_square(t1,t0); fe25519_square(t0,t1); }
	/* 2^50 - 2^0 */ fe25519_mul(z2_50_0,t0,z2_10_0);

	/* 2^51 - 2^1 */ fe25519_square(t0,z2_50_0);
	/* 2^52 - 2^2 */ fe25519_square(t1,t0);
	/* 2^100 - 2^50 */ for (i = 2;i < 50;i += 2) { fe25519_square(t0,t1); fe25519_square(t1,t0); }
	/* 2^100 - 2^0 */ fe25519_mul(z2_100_0,t1,z2_50_0);

	/* 2^101 - 2^1 */ fe25519_square(t1,z2_100_0);
	/* 2^102 - 2^2 */ fe25519_square(t0,t1);
	/* 2^200 - 2^100 */ for (i = 2;i < 100;i += 2) { fe25519_square(t1,t0); fe25519_square(t0,t1); }
	/* 2^200 - 2^0 */ fe25519_mul(t1,t0,z2_100_0);

	/* 2^201 - 2^1 */ fe25519_square(t0,t1);
	/* 2^202 - 2^2 */ fe25519_square(t1,t0);
	/* 2^250 - 2^50 */ for (i = 2;i < 50;i += 2) { fe25519_square(t0,t1); fe25519_square(t1,t0); }
	/* 2^250 - 2^0 */ fe25519_mul(t0,t1,z2_50_0);

	/* 2^251 - 2^1 */ fe25519_square(t1,t0);
	/* 2^252 - 2^2 */ fe25519_square(t0,t1);
	/* 2^253 - 2^3 */ fe25519_square(t1,t0);
	/* 2^254 - 2^4 */ fe25519_square(t0,t1);
	/* 2^255 - 2^5 */ fe25519_square(t1,t0);
	/* 2^255 - 21 */ fe25519_mul(r,t1,z11);
}

void printArray(const char* label, unsigned char* array, size_t size)
{
  Serial.print(label);
  Serial.print(": ");
  for (size_t i = 0; i < size; i++)
  {
    Serial.print(array[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}