#include <esp32/rom/ets_sys.h>

#define TEST_PIN 4
volatile uint32_t dummy = 0; // Variable volatile pour éviter l'optimisation

// Adresses des registres GPIO (ESP32)
#define GPIO_OUT_REG (DR_REG_GPIO_BASE + 0x04)
#define GPIO_IN_REG  (DR_REG_GPIO_BASE + 0x3C)

//------------------------------------------------------------------
// Mesure les cycles d'une opération d'écriture
//------------------------------------------------------------------
void testWriteCycles() {
  uint32_t start, end, totalArduino = 0, totalRegistre = 0;
  const uint32_t iterations = 1000;

  // Test digitalWrite
  for (uint32_t i = 0; i < iterations; i++) {
    start = esp_cpu_get_cycle_count();
    digitalWrite(TEST_PIN, HIGH);
    end = esp_cpu_get_cycle_count();
    totalArduino += (end - start);
    
    digitalWrite(TEST_PIN, LOW); // Reset
  }

  // Test écriture directe du registre
  volatile uint32_t* gpio_out = (volatile uint32_t*)GPIO_OUT_REG;
  for (uint32_t i = 0; i < iterations; i++) {
    start = esp_cpu_get_cycle_count();
    *gpio_out |= (1 << TEST_PIN);  // Set HIGH
    end = esp_cpu_get_cycle_count();
    totalRegistre += (end - start);
    
    *gpio_out &= ~(1 << TEST_PIN); // Reset
  }

  Serial.printf("[WRITE] digitalWrite: %u cycles | Registre: %u cycles\n", 
               totalArduino/iterations, 
               totalRegistre/iterations);
}

//------------------------------------------------------------------
// Mesure les cycles d'une opération de lecture
//------------------------------------------------------------------
void testReadCycles() {
  uint32_t start, end, totalArduino = 0, totalRegistre = 0;
  const uint32_t iterations = 1000;

  // Test digitalRead
  for (uint32_t i = 0; i < iterations; i++) {
    start = esp_cpu_get_cycle_count();
    dummy = digitalRead(TEST_PIN); // dummy pour consommation
    end = esp_cpu_get_cycle_count();
    totalArduino += (end - start);
  }

  // Test lecture directe du registre
  volatile uint32_t* gpio_in = (volatile uint32_t*)GPIO_IN_REG;
  for (uint32_t i = 0; i < iterations; i++) {
    start = esp_cpu_get_cycle_count();
    dummy = (*gpio_in >> TEST_PIN) & 0x1; // dummy pour consommation
    end = esp_cpu_get_cycle_count();
    totalRegistre += (end - start);
  }

  Serial.printf("[READ]  digitalRead: %u cycles | Registre: %u cycles\n\n", 
               totalArduino/iterations, 
               totalRegistre/iterations);
}

//------------------------------------------------------------------
void setup() {
  Serial.begin(115200);
  pinMode(TEST_PIN, OUTPUT);
}

void loop() {
  testWriteCycles();
  testReadCycles();
  delay(2000);
}