// Uncomment this if when you copy this code to run it on HW
//#define RUN_ON_HW

#ifdef RUN_ON_HW
#define Serial1 Serial
#endif

//Understanding memory map of RP2040
// Ref: RP2040 datasheet: 2.2 Address Map (page 25/647)
// XIP: 2.6.3 Flash: (page 150/647)
// GPIO: 2.19. GPIO --> 2.3.1.2. GPIO Control
// GPIO_OUT --> SIO: GPIO_OUT Register
// Offset: 0x010
// SIO base address: 2.3.1.7. List of Registers --> 0xD0000000

// Ref: Section 2.3.1.7 List of Registers (from RP2040 Datasheet)

#define BIT_20_MASK 0x100000

const unsigned int sio_base = 0xD0000000;
const unsigned int cpuid_offset = 0x00;
const unsigned int gpio_in_offset = 0x04;
const unsigned int gpio_out_offset = 0x010;
const unsigned int gpio_out_xor_offset = 0x01C;

unsigned int* cpuid_ptr =
                   (unsigned int *) (sio_base | cpuid_offset);

unsigned int* gpio_in_ptr =
                   (unsigned int *) (sio_base | gpio_in_offset);                   

unsigned int* gpio_out_ptr =
                   (unsigned int *) (sio_base | gpio_out_offset);

unsigned int* gpio_out_xor_ptr =
                   (unsigned int *) (sio_base | gpio_out_xor_offset);

static int myNum =0;
const int myConstNum = 100;
int cpuid;

#define GPIO_2 2
#define GPIO_3 3
#define GPIO_20 20
#define GPIO_5 5

void setup() {

  int myLocalVar = 10;

#ifdef RUN_ON_HW
  delay(10000);
#endif  

  // put your setup code here, to run once:
  Serial1.begin(115200);
  Serial1.println("Hello, Raspberry Pi Pico!");

  pinMode(GPIO_2, OUTPUT);
  pinMode(GPIO_3, OUTPUT);
  pinMode(GPIO_20, INPUT);

  pinMode(GPIO_5, OUTPUT);

  *gpio_out_ptr = (unsigned int)0b01100;

}


void loop() {

  bool in_val;
  unsigned int gpio_cur_state;

  delay(5000); 

  cpuid = *cpuid_ptr;
  Serial1.println("The CPUID value:");
  Serial1.println(cpuid);

  //*gpio_out_ptr = (unsigned int)0b01100;

  *gpio_out_xor_ptr = (unsigned int)0b01100;
  
  // put your main code here, to run repeatedly:

  delay(5000);
  //*gpio_out_ptr = (unsigned int)0;

  *gpio_out_xor_ptr = (unsigned int)0b01100;

  in_val = digitalRead(GPIO_20);

  // To extract the Bit 20 value from 32 bits GPIO state
  in_val = ((*gpio_in_ptr) & BIT_20_MASK) >> 20;

  Serial1.println("Value of in_val");
  Serial1.println(in_val);

 // digitalWrite(GPIO_5, in_val); // 5th pin of GPIO

  // Only modify the fifth GPIO value as per input received
  // Keep the current values on other GPIO pins (2 and 3) untouched
  
   gpio_cur_state = *gpio_in_ptr;

  *gpio_out_ptr = (unsigned int) ( (gpio_cur_state & 0xFFFFFFDF)
                                         | (in_val << GPIO_5));

}