//Code by: sanelss@gmail.com

#include <Wire.h>

byte _address;
byte _register;
byte _value;
String _inbuffer;

void setup(){
  _address=0;
  _inbuffer="";

  Serial.begin(115200);
  while(!Serial);

  Wire.begin();

  printMainMenu();
}

void loop(){
  if(Serial.available()){
    char c=Serial.read();
    Serial.println(c);
    switch(c-'0'){
      case 1:
        _address=promptInput("Device Address");
        Serial.print("Device Address changed to: ");
        printAllTypes(_address);
        Serial.print("\nEnter Menu Item:\n>");
        break;
      case 2:
        _register=promptInput("Rgister Address");
        _value=promptInput("Value");
        writeTo(_register,_value);
        Serial.print("Register ");
        printAllTypes(_register);
        Serial.print("  =  ");
        printAllTypes(readRegister(_register));
        Serial.print("\nEnter Menu Item:\n>");
        break;
      case 3:
        _register=promptInput("Rgister Address");
        Serial.print("Register 0x");
        Serial.print(_register,HEX);
        Serial.print(": ");
        printAllTypes(readRegister(_register));
        Serial.print("\nEnter Menu Item:\n>");
        break;
      default:
        Serial.println("\n\nInvalid Input");
        printMainMenu();
        break;
    }
  }
}

void printMainMenu(){
  Serial.println("-------------------------------------");
  Serial.println("Main Menu:");
  Serial.println("1. Enter/change Device Address");
  Serial.println("2. Write Register");
  Serial.println("3. Read Register");
  Serial.println("4. Read Register X times at Y Interval");
  Serial.println("5. Read X Registers starting at register Y");
  Serial.println("-------------------------------------");
  Serial.print("Current device: ");
  printAllTypes(_address);
  Serial.print("\nEnter Menu Item:\n>");
}









byte readRegister(byte reg){
  byte _buff[1];
  readFrom(reg,1,_buff);
  return _buff[0];
}

byte promptInput(String msg){
  Serial.print("Enter ");
  Serial.print(msg);
  Serial.print(": (Example: 0xCA,0XCA,0xca,0Xca, 0b11001010, 0B11001010, 202)\n>");
  while(!Serial.available());
  _inbuffer="";
  while(Serial.available()>0)
     _inbuffer+=(char)Serial.read();
  Serial.println(_inbuffer);
  return getValueFromString(_inbuffer);
}


void printAllTypes(byte data){
  Serial.print("0x");
  Serial.print(data,HEX);
  Serial.print(", 0b");
  Serial.print(data,BIN);
  Serial.print(", ");
  Serial.println(data);
}

byte getValueFromString(String s){
  byte ret;
  if(s[0]=='0'&&(s[1]=='x'||s[1]=='X')){
    ret=(strToInt(s[2])*16+strToInt(s[3]));
  }
  else if(s[0]=='0'&&(s[1]=='b'||s[1]=='B')){
    ret=0;
    for(int i=0;i<s.length()&&i<8;i++){
      ret|=((s[s.length()-1-i]=='1')? 1:0)<<i;
    }
  }
  else{
    char buf[4];
    s.toCharArray(buf,sizeof(buf));
    ret=atoi(buf);
  }
  return ret;
}

byte strToInt(char c){
  byte b=c-'0';
  if(b>48&&b<55)
    b-=39;
  else if(b>9&&b<23)
    b-=7;
  else if(b<0||b>9)
    b=0;
  return b;
}

void writeTo(byte reg, byte val) {
  Wire.beginTransmission(_address);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}


void readFrom(byte reg, int num, byte _buff[]) {
  Wire.beginTransmission(_address);
  Wire.write(reg);
  Wire.endTransmission();

  Wire.beginTransmission(_address);
  Wire.requestFrom((uint8_t)_address,(uint8_t)num);

  int i = 0;
  while(Wire.available()){
    _buff[i] = Wire.read();
    i++;
  }
  Wire.endTransmission();
}