#include<Wire.h>

#define SLAVE_ADDRESS 0x

#define SONIC_STATUS_CMD "sonic_dist"

#define TIMEOUT 
#define BYTE_ARR_SIZE 
#define SERIAL_BITS_NUM 
#define EOS "'\0'"
#define EOL '\n'
#define MAX_TASKS 
#define length(arr) sizeof(arr)/sizeof(*arr)

template<class T> 
inline Print &operator <<(Print &obj, T arg) { obj.print(arg); return obj; }
HardwareSerial &operator >>(HardwareSerial &obj, String &arg) { arg=obj.readString(); return obj; }
HardwareSerial &operator >>(HardwareSerial &obj, int &arg) { arg=(int) obj.parseInt(); return obj; }
HardwareSerial &operator >>(HardwareSerial &obj, long &arg) { arg=obj.parseInt(); return obj; }
HardwareSerial &operator >>(HardwareSerial &obj, char &arg) { arg=obj.read(); return obj;}
HardwareSerial &operator >>(HardwareSerial &obj, float &arg) { arg=obj.parseFloat(); return obj; }

class TaskManager
{
private:
    int ticks;
    unsigned long Timeout[MAX_TASKS][2] = {0};
    void (*Tasks[MAX_TASKS])() = {0};

public:
    int tick()
    {
        unsigned long time_now = millis();
        for(int i = 0; i < MAX_TASKS; i++)
        {
            if(Tasks[i] != 0 && time_now >= Timeout[i][1])
            {
                Timeout[i][1] = time_now + Timeout[i][0];
                (*Tasks[i])();
            }
        }
    }

    bool AddTask(void (*fun)(), unsigned long timeout)
    {
        int available = -1;
        for(int i = 0; i < MAX_TASKS; i++)
        {
            if(Tasks[i] == fun)
            {
                return false;
            }
            else
            {
                if(Tasks[i] == 0 && available == -1)
                    available = i;
            }
        }
        if(available != -1 && timeout != 0)
        {
            Tasks[available]=fun;
            Timeout[available][0] = timeout;
            Timeout[available][1] = millis() + timeout;
            return true;
        }
        return false;
    }

    bool RemoveTask(void (*fun)())
    {
        for(int i = 0; i < MAX_TASKS; i++)
        {
            if(Tasks[i] == fun)
            {
                Tasks[i] = 0;
                return true;
            }
        }
        return false;
    }

    bool ChangeTimeout(void (*fun)(), unsigned long timeout)
    {
        for(int i = 0; i < MAX_TASKS; i++)
        {
            if(Tasks[i] == fun)
            {
                Timeout[i][0] = timeout;
                Timeout[i][1] = millis() + timeout;
                return true;
            }
        }
        return false;
    }
};

uint8_t Buffer[BYTE_ARR_SIZE];
int buff_it;
TaskManager manager;
bool sonic_cmd = false;

void setup()
{
    Serial.begin(SERIAL_BITS_NUM);
    Wire.begin();
    manager.AddTask(WaitCmd, TIMEOUT);
    manager.AddTask(RequestData, TIMEOUT);
}

void WaitCmd()
{
    if(!Serial.available())
        return;

    String cmd;
    Serial >> cmd;

    if(cmd.equals(SONIC_STATUS_CMD))
    {
        sonic_cmd = true;
    }
    else
    {
        Serial <<"Unknown Command." << EOL;
    }
}

int Create_Int(const uint8_t Byte_hi, const uint8_t Byte_lo)
{
    return (int)(( Byte_hi << 8 ) | Byte_lo & 0x00FF );
}

void RequestData()
{
    if(!sonic_cmd)
        return;

    sonic_cmd = false;

    Wire.requestFrom(SLAVE_ADDRESS, BYTE_ARR_SIZE);

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

    int distance = Create_Int(Buffer[0], Buffer[1]);
    Serial <<"Sonic Distance Received from Slave is : " << distance << EOL;
}

void loop()
{
    manager.tick();
}