#include "mbed.h"
#include <VL6180x.h>

#define VL6180X_ADDRESS 0x29
 
VL6180xIdentification identification;

// mbed uses 8bit addresses shift address by 1 bit left
VL6180x sensor(p28, p27, VL6180X_ADDRESS<<1);
//CAN can(p30, p29); //rd, td
SPI spi(p5,p6,p7);
DigitalOut cs(p8);

int main()
{

    CANMessage messageOut; //formatting CANMessage
    
    cs = 1;
    
    messageOut.format = CANStandard;
    messageOut.id = 0x720;
    messageOut.len = 1;

    uint8_t distance; //dataToSend
    
    spi.format(8,0);
    spi.frequency(1000000);
    //can.reset(); moved to while loop to reset 
    cs=0;
    
    wait_ms(10); // delay .1s

    sensor.getIdentification(&identification); // Retrieve manufacture info from device memory
    printIdentification(&identification); // Helper function to print all the Module information

    if(sensor.VL6180xInit() != 0) {
        printf("FAILED TO INITALIZE\n"); //Initialize device and check for errors
    };

    sensor.VL6180xDefautSettings(); //Load default settings to get started.

    wait_ms(20); // delay

    while(1) {


        distance = sensor.getDistance();  
          
        printf("%u\n", distance); 
        
        messageOut.data[0] = distance; 
        
        spi.write(messageOut.data[0]); //in my CAN code it was 'can.write(messageOut);'

        wait_ms(10); //100hz
        
        //can.reset(); 

    }
}