#include <Arduino.h>
#include <BigNumber.h>
//------------------------------------------------------------------------------------------------------------
void add32(uint8_t *r, const uint8_t *a, const uint8_t *b)
{
  uint16_t carry = 0;
  for (int i = 0; i < 32; i++)
  {
    uint16_t sum = (uint16_t)a[i] + (uint16_t)b[i] + carry;
    r[i] = sum & 0xFF; // Store the lower 8 bits in the result
    carry = (sum >> 8) & 1;  // Carry is the upper 8 bits
  }
  r[32] = carry; // Store the final carry in the next byte
}
//------------------------------------------------------------------------------------------------------------
// Function to convert a 32-byte array to a BigNumber
BigNumber arrayToBigNumber(const uint8_t* bytes, size_t length) {
    BigNumber result = "0";
    BigNumber base = "1";

    for (int i = 0; i < length; i++) {
        BigNumber byteValue = BigNumber(bytes[i]);
        result = result + byteValue * base;
        base = base * 256;
    }

    return result;
}
//------------------------------------------------------------------------------------------------------------
int findFirstOneBitPosition(const uint8_t* bytes, size_t length)
{
  // Inverting all the bytes
  uint8_t twoscomp[32] = {0};
  uint8_t isolatedBit [32] = {0};
  for (int i = 31; i >= 0; --i)
  {
    twoscomp[i] = ~bytes[i];
  }

  Serial.print("Inverting bytes = ");
  for (int i = 31; i >= 0; --i)
  {
    Serial.print(twoscomp[i], BIN);
    Serial.print(" ");
  }
  Serial.println(" ");
  
  add32(twoscomp, twoscomp, 1)
  
  Serial.print("Adding 1 to the inerted bytes to get the two's complement = ");
  for (int i = 31; i >= 0; --i)
  {
    Serial.print(twoscomp[i], BIN);
    Serial.print(" ");
  }
  Serial.println(" ");
  
  for (int i = 31; i >= 0; --i)
  {
    isolatedBit [i] = twoscomp[i] & ~bytes[i];
  }

  Serial.print("Step 2: Isolated Bit = ");
  for (int i = 31; i >= 0; --i)
  {
    Serial.print(isolatedBit[i], BIN);
    Serial.print(" ");
  }
  Serial.println(" ");

  BigNumber decimalNumber = arrayToBigNumber(isolatedBit, 32);

  Serial.print("Decimal number: ");
  Serial.println(decimalNumber);
  
  // Step 3: Find the position of the isolated bit (1-based index)
  // Use precomputed log2 value (1.44269504089) for efficiency
  int position = round((log(decimalNumber) * 1.44269504089));
  Serial.print("Step 3: Position of isolated bit = ");
  Serial.println(position);

  return position;
}

void setup()
{
  // Initialize serial communication at 9600 bits per second
  Serial.begin(9600);
  BigNumber::begin();  // Initialize the BigNumber library
  // Example 32-byte array representing a number
  // The number should be saved and printed in using Little Endian method ----------> This is an important note, which made to us a waste a lot of times
uint8_t binaryArray[32] = {0x00, 0x00, 0x00, 0x00, 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};

  
  Serial.print("Binary Array:");
  for (int i = 31; i >= 0; --i)
  {
    Serial.print(binaryArray[i], BIN);
    Serial.print(" ");
  }
  Serial.println(" ");
  
  // Find the position of the first 1 bit from the right
  int position = findFirstOneBitPosition(binaryArray, 32);

  // Print the position
  Serial.print("The position of the first 1 bit (from the right) is : ");
  Serial.println(position);
}

void loop()
{
  // Nothing to do here
}