#define ESP32_S3_WROOM_1

/***********************************/
/***** ESP32 LAYOUT DEFINITION *****/
/***** vvvvvvvvvvvvvvvvvvvvvvv *****/
#ifdef ESP32_S3_WROOM_1

// Pin numbers represent GPIOnum

#define DIO1_PIN   4   /* GPIB 1  */
#define DIO2_PIN   5   /* GPIB 2  */
#define DIO3_PIN   6   /* GPIB 3  */
#define DIO4_PIN   7   /* GPIB 4  */
#define DIO5_PIN  15   /* GPIB 13 */
#define DIO6_PIN  16   /* GPIB 14 */
#define DIO7_PIN  17   /* GPIB 15 */
#define DIO8_PIN  18   /* GPIB 16 */

#define IFC_PIN   10   /* GPIB 9  */
#define NDAC_PIN  11   /* GPIB 8  */
#define NRFD_PIN  12   /* GPIB 7  */
#define DAV_PIN   13   /* GPIB 6  */
#define EOI_PIN   14   /* GPIB 5  */

#define SRQ_PIN    9   /* GPIB 10 */
#define REN_PIN   47   /* GPIB 17 */
#define ATN_PIN   48   /* GPIB 11 */

#endif // ESP32_S3_WROOM_1
/***** ^^^^^^^^^^^^^^^^^^^^^^^ *****/
/***** ESP32 LAYOUT DEFINITION *****/
/***********************************/

typedef struct gpioregister_t {
  uint32_t reg0 = 0;
  uint32_t reg1 = 0;
};



//union gpioregister_t {
//  pinbanks_t reg;
//  uint64_t all = 0;
//};


const uint8_t databus[8] = { DIO1_PIN, DIO2_PIN, DIO3_PIN, DIO4_PIN, DIO5_PIN, DIO6_PIN, DIO7_PIN, DIO8_PIN };
const uint8_t ctrlreg[8] = { IFC_PIN, NDAC_PIN, NRFD_PIN, DAV_PIN, EOI_PIN, REN_PIN, SRQ_PIN, ATN_PIN };

gpioregister_t gpioDbMask;
gpioregister_t gpioCtrlMask;


void printRegMaskBin(gpioregister_t& gpioreg){
  Serial.print(gpioreg.reg1, BIN);
  Serial.println(gpioreg.reg0, BIN);
}



void genGpioMask(gpioregister_t& gpioreg, const uint8_t buspins[], uint8_t bitmask) {
  for (uint8_t i=0; i<8; i++) {
    if (bitmask & (1 << i)) {
      if (buspins[i] > 31) {
        gpioreg.reg1 |= ( 1ULL << (buspins[i]-32) );
      }else{
        gpioreg.reg0 |= ( 1ULL << buspins[i] );
      }
    }
  }
}


void gpioSetPullupsMasked(const uint8_t bus[], uint8_t mask, bool enable){
  Serial.println("Pullup GPIOs:");
  for (uint8_t i=0; i<8; i++) {
    if (mask & (1<<i)) {
      if (enable) {
        if ( (gpio_num_t)bus[i] ){
          Serial.print("EN: ");
          Serial.println(bus[i]);
          gpio_set_pull_mode((gpio_num_t)bus[i], GPIO_PULLUP_ONLY);
          gpio_pullup_en( (gpio_num_t)bus[i] );
        }
      }else{
        if ( (gpio_num_t)bus[i] ) {
          Serial.print("DI: ");
          Serial.println(bus[i]);
          gpio_pullup_dis( (gpio_num_t)bus[i] );
        }
      }
    }
  }
}


/**** Set the GPIB data bus to input pullup *****/
void readyGpibDbus(uint8_t state = INPUT_PULLUP) {
  // Set pins to OUTPUT
  if (state == OUTPUT) {
    // Disable pullups
    gpioSetPullupsMasked(databus, 0xFF, false);
    // Set to output
    REG_WRITE( GPIO_ENABLE_W1TS_REG, gpioDbMask.reg0 );
    REG_WRITE( GPIO_ENABLE1_W1TS_REG, gpioDbMask.reg1 );
    // Set to HIGH
    REG_WRITE( GPIO_OUT_W1TS_REG, gpioDbMask.reg0 );
    REG_WRITE( GPIO_OUT1_W1TS_REG, gpioDbMask.reg1 );

//Serial.println("Dbus: ");
//Serial.print( REG_READ( GPIO_OUT1_REG), BIN );
//Serial.println( REG_READ( GPIO_OUT_REG), BIN );

    return;
  }
  // Set pins to INPUT_PULLUP
  REG_WRITE( GPIO_ENABLE_W1TC_REG, gpioDbMask.reg0 );
  REG_WRITE( GPIO_ENABLE1_W1TC_REG, gpioDbMask.reg1 );
  // Enable pullups
  gpioSetPullupsMasked(databus, 0xFF, true);

}


/***** Read the GPIB data bus wires to collect the byte of data *****/
uint8_t readGpibDbus() {
  uint64_t gpioall = 0;
  // Read the byte of data on the bus`
  gpioall = REG_READ(GPIO_IN_REG);
  gpioall |= ( REG_READ(GPIO_IN1_REG) << 32 );

  uint8_t result;
  for (uint8_t i=0; i<8; i++) {
    if ( gpioall & (1ULL<<databus[i]) ) result = (result | (1U<<i));
  }
  return ~result;
}



/***** Set the GPIB data bus to output and with the requested byte *****/
void setGpibDbus(uint8_t db) {

  gpioregister_t gpioDbM;
  gpioregister_t gpioDbV;

  // Create masks
  genGpioMask(gpioDbM, databus, 0xFF);
  genGpioMask(gpioDbV, databus, db);

Serial.println("gpioDbC: ");
printRegMaskBin(gpioDbM);

Serial.println("gpioDbS: ");
printRegMaskBin(gpioDbV);


  // Clear bits
  REG_WRITE( GPIO_OUT_W1TS_REG, gpioDbM.reg0 );
  REG_WRITE( GPIO_OUT1_W1TS_REG, gpioDbM.reg1 );
  // Set required value
  if (~db) {
    REG_WRITE( GPIO_OUT_W1TC_REG, gpioDbV.reg0 );
    REG_WRITE( GPIO_OUT1_W1TC_REG, gpioDbV.reg1 );
  }
}



/***** Read the GPIB data bus wires to collect the byte of data *****/
uint8_t readGpibCtrlbus() {
  uint64_t gpioall = 0;
  uint8_t result = 0;

  // Read the byte of data on the bus`
  gpioall = REG_READ(GPIO_IN_REG);
  gpioall |= ( REG_READ(GPIO_IN1_REG) << 32 );

  for (uint8_t i=0; i<8; i++) {

  Serial.println("InReg: ");
  Serial.println(gpioall, BIN);

    if ( gpioall & (1ULL<<ctrlreg[i]) ) result |= (1U<<i);
    
    Serial.print("R[i]: " );
    Serial.println(ctrlreg[i]);
    Serial.print("T: " );
    Serial.println(1ULL<<ctrlreg[i], BIN);
    Serial.print("I: " );
    Serial.println(1<<i, BIN);
    Serial.print("R: " );
    Serial.println(result, BIN);

  }

  return ~result;
}



void setGpibCtrlState(uint8_t bits, uint8_t mask){

  uint8_t hbits = bits & mask;
  uint8_t lbits = ~(bits | ~mask);

  if (hbits) {
    gpioregister_t gpioHbits;
    genGpioMask(gpioHbits, ctrlreg, hbits);

Serial.print("Hbitmask: ");
printRegMaskBin(gpioHbits);

    if (gpioHbits.reg0) REG_WRITE( GPIO_OUT_W1TS_REG, gpioHbits.reg0 );
    if (gpioHbits.reg1) REG_WRITE( GPIO_OUT1_W1TS_REG, gpioHbits.reg1 );
  }
  if (lbits){
    gpioregister_t gpioLbits;
    genGpioMask(gpioLbits, ctrlreg, lbits);

Serial.print("Lbitmask: ");
printRegMaskBin(gpioLbits);

    if (gpioLbits.reg0) REG_WRITE( GPIO_OUT_W1TC_REG, gpioLbits.reg0 );
    if (gpioLbits.reg1) REG_WRITE( GPIO_OUT1_W1TC_REG, gpioLbits.reg1 );
  }
}


void setGpibCtrlDir(uint8_t bits, uint8_t mask){

  uint8_t obits = bits & mask;
  uint8_t ibits =  ~(bits | ~mask);

Serial.print("obits: ");
Serial.println(obits, BIN);
Serial.print("ibits: ");
Serial.println(ibits, BIN);

  if (obits){
    // Derrive register output bits
    gpioregister_t gpioObits;
    genGpioMask(gpioObits, ctrlreg, obits);

Serial.print("gpioObits: ");
printRegMaskBin(gpioObits);

    // Disable pullups
    gpioSetPullupsMasked(ctrlreg, obits, false);

    // Set GPIOs to outputs
    if (gpioObits.reg0) REG_WRITE( GPIO_ENABLE_W1TS_REG, gpioObits.reg0 );
    if (gpioObits.reg1) REG_WRITE( GPIO_ENABLE_W1TS_REG, gpioObits.reg1 );
  }

  if (ibits){
    // Derrive register input bits
    gpioregister_t gpioIbits;
    genGpioMask(gpioIbits, ctrlreg, ibits);

Serial.print("goioIbits: ");
printRegMaskBin(gpioIbits);

    // Set GPIOs to inputs
    if (gpioIbits.reg0) REG_WRITE( GPIO_ENABLE_W1TC_REG, gpioIbits.reg0 );
//    if (gpioIbits.reg0) REG_WRITE( GPIO_IN_REG, ~gpioIbits.reg0 );
    if (gpioIbits.reg1) REG_WRITE( GPIO_ENABLE_W1TC_REG, gpioIbits.reg1 );
//    if (gpioIbits.reg1) REG_WRITE( GPIO_IN1_REG, ~gpioIbits.reg1 );

Serial.print("goioIreg: ");
Serial.println(REG_READ(GPIO_IN_REG), BIN);
Serial.println(REG_READ(GPIO_IN1_REG), BIN);

    // Set pullups
    gpioSetPullupsMasked(ctrlreg, ibits, true);
  }
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");

//  pinMode(DIO2_PIN, OUTPUT);
//  digitalWrite(DIO2_PIN, HIGH);

  char version[50] = "AR488";

  genGpioMask(gpioDbMask, databus, 0xFF);
  genGpioMask(gpioCtrlMask, ctrlreg, 0xFF);
/*
  Serial.print("DbMask: ");
  Serial.println(gpioDbMask.all, BIN);
  Serial.print("CtrlMask: ");
  Serial.println(gpioCtrlMask.all, BIN);
*/
  // Read outputs test
  readyGpibDbus(OUTPUT);
//  setGpibDbus(0xAA);
//  delay(3000);
//  setGpibDbus(0x55);

  // Inputs test
  setGpibCtrlDir(0, 0xFF);

//  pinMode(8, INPUT_PULLUP);

}

void loop() {
  // put your main code here, to run repeatedly:
/*
  uint8_t result = 0;


  result = readGpibCtrlbus();
//  setGpibDbus(result);
  Serial.println("GPIO: ");
  Serial.println( REG_READ(GPIO_IN_REG), BIN );
  Serial.println( REG_READ(GPIO_IN1_REG), BIN );
  Serial.println("Result: ");
  Serial.println( result, BIN );

  setGpibDbus(result);

  delay(3000); // this speeds up the simulation

*/
}