#include <Wire.h>

int dev_Addr_rd = 0xAE; // Device address ; 0xA6 for write ; 0xAE for read
int dev_Addr_wr = 0xA6; // Device address ; 0xA6 for write ; 0xAE for read

////Address in .rpd file, but remember the address is offset by 0x10000 means the below address is actually 0x024180 in rpd file.
#define avlon_Addr0 0x00 // Hexadecima address for the internal register.
#define avlon_Addr1 0x03 // Hexadecima address for the internal register.
#define avlon_Addr2 0xC8 // Hexadecima address for the internal register.
#define avlon_Addr3 0x20 // Hexadecima address for the internal register.

//Data to be written
#define avl_wr_d0 0x33 
#define avl_wr_d1 0x22 
#define avl_wr_d2 0x3A 
#define avl_wr_d3 0x12


void setup() {
  Wire.begin();        // join i2c bus (address optional for master)
  Serial.begin(9600);  // start serial for output
}


void loop () {

   
  Serial.print("------------------------\n");
   Serial.print(" Write\n");
   avalon_wr();
   delay(500);

  Serial.print("------------------------\n");
   Serial.print(" Read\n");
   avalon_rd();
   delay(500);
  Serial.print("============END=======================\n");
}


//Read Loop------------------------------------------------
void avalon_rd() {

byte a,b,c,d,a1,b1,c1,d1;
  Wire.beginTransmission(dev_Addr_rd>>1); // Begin transmission 
  //Ask the particular registers for data
  Wire.write(avlon_Addr0);
  Wire.write(avlon_Addr1);
  Wire.write(avlon_Addr2);
  Wire.write(avlon_Addr3);

        
  Wire.endTransmission(); // Ends the transmission and transmits the data from the registers

  
  Wire.requestFrom(dev_Addr_rd>>1, 4);    // request 1 bytes from dev_Addr

  a1 = Wire.read();
  b1 = Wire.read();
  c1 = Wire.read();
  d1 = Wire.read();

  a = flipByte(a1);
  b = flipByte(b1);
  c = flipByte(c1);
  d = flipByte(d1);

  Serial.print(a,HEX); Serial.print("  ");
  Serial.print(b,HEX); Serial.print("  ");
  Serial.print(c,HEX); Serial.print("  ");
  Serial.print(d,HEX); Serial.print("  ");
  Serial.print("\n");

//  while (Wire.available()) { // slave may send less than requested
//    int c = Wire.read(); // receive a byte 
//    Serial.println(c, HEX);         // print 
//  }

  delay(500);
}


//Write Loop------------------------------------------------
void avalon_wr() 
{
byte wr_a,wr_b,wr_c,wr_d;
byte wr_Array[4];
  
  wr_a = flipByte(avl_wr_d0);
  wr_b = flipByte(avl_wr_d1);
  wr_c = flipByte(avl_wr_d2);
  wr_d = flipByte(avl_wr_d3);

//  wr_a = (avl_wr_d0);
//  wr_b = (avl_wr_d1);
//  wr_c = (avl_wr_d2);
//  wr_d = (avl_wr_d3);

wr_Array[0] = wr_a;
wr_Array[1] = wr_b;
wr_Array[2] = wr_c;
wr_Array[3] = wr_d;


  Wire.beginTransmission(dev_Addr_wr>>1); // Begin transmission 
  //Writing 4 bytes of avalon address
  Wire.write(avlon_Addr0);
  Wire.write(avlon_Addr1);
  Wire.write(avlon_Addr2);
  Wire.write(avlon_Addr3);

////Writing 4 bytes of data
//  Wire.write(wr_a);
//  Wire.write(wr_b);
//  Wire.write(wr_c);
//  Wire.write(wr_d);

 Wire.write(wr_Array, 4);

 Wire.endTransmission(); // Ends the transmission and transmits the data from the registers
  delay(500);
}


byte flipByte(byte c){
  char r=0;
  for(byte i = 0; i < 8; i++){
    r <<= 1;
    r |= c & 1;
    c >>= 1;
  }
  return r;
}