//Code by: sanelss@gmail.com

#include <Wire.h>

byte _address;
byte _register;
byte _value;

uint32_t _time;
uint32_t _oldtime;
uint16_t _interval;

uint16_t _bytecount;

String _inbuffer;
char c;

void setup(){
  _address=0;
  _register=0;
  _value=0;

  _time=0;
  _oldtime=0;
  _interval=0;

  _bytecount=0;

  _inbuffer="";

  Serial.begin(115200);
  while(!Serial);
  Wire.begin();
  printMainMenu();
}
void loop(){
  if(_interval!=0){
    _time=millis();
    if(_time>_oldtime+_interval){
      _oldtime=_time;
      Serial.print("***Register ");
      printAllTypes(_register);
      Serial.print("  =  ");
      printAllTypes(readRegister(_register));
      Serial.println("");
    }
  }

  if(Serial.available()){
    c=Serial.read();
    if(c==10||c==13){
       Serial.println("");
       processBuffer();
    }
    else{
      Serial.print(c);
      _inbuffer+=c;
    }
  }
}



void printMainMenu(boolean printcursor){
  Serial.println("-------------------------------------");
  Serial.println("Main Menu:");
  Serial.println("0. Clear tasks & Show Menu");
  Serial.println("1. Enter/change Device Address");
  Serial.println("2. Write Register");
  Serial.println("3. Read Register");
  Serial.println("4. Read Register X at Y ms Interval");
  Serial.println("5. Read Y bytes from Register X");
  Serial.println("6. Scan for i2c devices(7-bit address)");
  Serial.println("*. Invalid options show menu");
  Serial.println("-------------------------------------");
  Serial.println("Direct commands:");
  Serial.println("read register   (read 0x00)");
  Serial.println("read register,interval   (read 0x00,250)");
  Serial.println("write register,value  (write 0x1F,42)");
  Serial.println("cls      - bunch of newline to clear txt");
  Serial.println("-------------------------------------");
  Serial.print("Current device: ");
  printAllTypes(_address);
  if(printcursor)
    printCursor();
}
void printCursor(){
  Serial.println("");
  Serial.println("");
  Serial.println("Enter Menu Item or Direct Command:");
  Serial.print(">");
}


void processBuffer(){
  _inbuffer.trim();
  if(_inbuffer=="0"){
    _interval=0;
    Serial.println("\n\n**********   Clearing tasks   **********");
    printMainMenu(false);
  }
  else if(_inbuffer=="1"){
    _address=promptInput("Device Address");
    Serial.print("***Device Address changed to: ");
    printAllTypes(_address);
  }
  else if(_inbuffer=="2"){
    _register=promptInput("Register Address");
    _value=promptInput("Value");
    writeTo(_register,_value);
    Serial.print("***Register ");
    printAllTypes(_register);
    Serial.print("  =  ");
    printAllTypes(readRegister(_register));
  }
  else if(_inbuffer=="3"){
    _register=promptInput("Register Address");
    Serial.print("***Register ");
    printAllTypes(_register);
    Serial.print("  =  ");
    printAllTypes(readRegister(_register));
  }
  else if(_inbuffer=="4"){
    _register=promptInput("Register Address");
    _interval=promptInput("Interval time in ms");
  }
  else if(_inbuffer=="5"){
    _register=promptInput("Register Address");
    _bytecount=promptInput("Ammount of bytes to read");
    byte _buff[_bytecount];
    readFrom(_register,_bytecount,_buff);

    for(int i=0;i<_bytecount;i++){
      printAllTypes(_register+i);
      Serial.print("  =  ");
      printAllTypes(_buff[i]);
      Serial.println("");
    }

  }
  else if(_inbuffer=="6"){
    for(byte i=1;i<127;i++){
      Wire.beginTransmission(i);
      byte e=Wire.endTransmission();
      if(e==0){
        Serial.println("");
        Serial.print("Found device at: ");
        printAllTypes(i);
      }
      else if(e==4){
        Serial.print("Error at: ");
        printAllTypes(i);
      }
    }
    Serial.println("\n***Scan finished");
  }
  else if(_inbuffer.startsWith("read ")){
    if(_inbuffer.indexOf(",")==-1){
      _register=getValueFromString(_inbuffer.substring(_inbuffer.indexOf(" ")+1));
      Serial.print("***Register ");
      printAllTypes(_register);
      Serial.print("  =  ");
      printAllTypes(readRegister(_register));
    }
    else{
      _register=getValueFromString(_inbuffer.substring(_inbuffer.indexOf(" ")+1,_inbuffer.indexOf(",")));
      _interval=getValueFromString(_inbuffer.substring(_inbuffer.indexOf(",")+1));
    }
  }
  else if(_inbuffer.startsWith("write ")){
    _register=getValueFromString(_inbuffer.substring(_inbuffer.indexOf(" ")+1,_inbuffer.indexOf(",")));
    _value=getValueFromString(_inbuffer.substring(_inbuffer.indexOf(",")+1));
    writeTo(_register,_value);
    Serial.print("***Register ");
    printAllTypes(_register);
    Serial.print("  =  ");
    printAllTypes(readRegister(_register));
  }
  else if(_inbuffer.startsWith("cls")){
    Serial.println("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
    printMainMenu(false);
  }
  else{
     Serial.println("\n\n**********   Invalid Input   **********");
     printMainMenu(false);
  }
  printCursor();
  _inbuffer="";
}















void printMainMenu(){
  printMainMenu(true);
}

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

uint16_t promptInput(String msg){
  uint16_t ret;
  Serial.print("***Enter ");
  Serial.print(msg);
  Serial.println(": (Ex: 0xCA,0XCA,0xca,0Xca,0b11001010,0B11001010,202)");
  Serial.print(">");
  c=0;
  _inbuffer="";
  while(!(c==10||c==13)){
    if(Serial.available()){
      c=Serial.read();
      if(!(c==10||c==13)){
        Serial.print(c);
        _inbuffer+=c;
      }
      else
        Serial.println("");
    }
  };
  ret=getValueFromString(_inbuffer);
  _inbuffer="";
  return ret;
}

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

uint16_t getValueFromString(String s){
  uint16_t ret;
  if(s.startsWith("0x")||s.startsWith("0X")){
    ret=(strToInt(s[2])*16+strToInt(s[3]));
  }
  else if(s.startsWith("0b")||s.startsWith("0B")){
    ret=0;
    for(int i=0;i<s.length()&&i<=8;i++){
      ret|=((s[s.length()-1-i]=='1')? 1:0)<<i;
    }
  }
  else{
    ret=s.toInt();
  }
  return ret;
}

byte strToInt(char c){
  byte b=0;
  if(c>='a'&&c<='f')
    b=c-'a'+10;
  else if(c>='A'&&c<='F')
    b=c-'A'+10;
  else if(c>='0'&&c<='9')
    b=c-'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();
}