#include "mbed.h"

I2C i2c(p9, p10);        // sda, scl
Serial pc(USBTX, USBRX); // tx, rx
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);

const int addr = 0x46; // define the I2C Address L
int mode = 1, mode0;
float echo, bai = 1.0;
int main() {
    char cmd[2];
    char cmdr[2];

    cmd[0] = 0x01;           // Power On
    i2c.write(addr, cmd, 1);
    cmd[0] = 0x07;           // Reset
    i2c.write(addr, cmd, 1);
    cmd[0] = 0x42;           // Typ Upper-bit
    i2c.write(addr, cmd, 1);
    cmd[0] = 0x65;           // Typ Lower-bit
    i2c.write(addr, cmd, 1);
    cmd[0] = 0x10;           // H-resolutiom Mode
    i2c.write(addr, cmd, 1);

    myled1 = 1;
    while(1) {
        myled2 = 1;
        wait(0.18);              // 
        if(mode == 0) wait(0.54);
        i2c.read(addr, cmdr, 2); // read the 2-byte echo result
        echo = ((cmdr[0] << 8) + cmdr[1]) / 1.2 * bai;

        mode0 = mode;
        if(mode == 0){
            if(echo >= 6000){
                mode = 2;
            }else if(echo >= 1000){
                mode = 1;
            }
        }
        if(mode == 1){
            if(echo < 100){
                mode = 0;
            }
            if(echo >= 30000){
                mode = 2;
            }
        }
        if(mode == 2){
            if(echo < 60){
                mode = 0;
            }else if(echo < 3000){
                mode = 1;
            }
        }
        if(mode != mode0){
            if(mode == 0){
                cmd[0] = 0x47;           // Max Upper-bit
                i2c.write(addr, cmd, 1);
                cmd[0] = 0x7E;           // Max Lower-bit
                i2c.write(addr, cmd, 1);
                cmd[0] = 0x11;           // H-resolutiom Mode2
                i2c.write(addr, cmd, 1);
                bai = (69.0 / 254.0) / 2.0;
            }
            if(mode == 1){
                cmd[0] = 0x42;           // Typ Upper-bit
                i2c.write(addr, cmd, 1);
                cmd[0] = 0x65;           // Typ Lower-bit
                i2c.write(addr, cmd, 1);
                cmd[0] = 0x10;           // H-resolutiom Mode
                i2c.write(addr, cmd, 1);
                bai = 1.0;
            }
            if(mode == 2){
                cmd[0] = 0x40;           // Min Upper-bit
                i2c.write(addr, cmd, 1);
                cmd[0] = 0x7F;           // Min Lower-bit
                i2c.write(addr, cmd, 1);
                cmd[0] = 0x10;           // H-resolutiom Mode
                i2c.write(addr, cmd, 1);
                bai = 69.0 / 31.0;
            }
            wait(2.0);              // 
            i2c.read(addr, cmdr, 2); // read the 2-byte echo result
            echo = ((cmdr[0] << 8) + cmdr[1]) / 1.2 * bai;
        }

        pc.printf("light = %.2f mode = %d bai = %.2f\r\n", echo, mode, bai);
        myled2 = 0;
        wait(0.82);
    }
}