#include "demo.hpp"

using namespace std;

int main(){

    // ----- Accelerometer -----
    auto accelerometer = ADXL345();
    // "HOT" trimming with current values of tilt
    // This emthod is not expected to be used in production
    //accelerometer.Trim_offsets();
    // Trimming with predefined values, based on previous measuments
    // Data for command below can be taken from command above
    accelerometer.Trim_offsets({0,0,0});

    // ----- Side laser -----
    auto side = Sick_side();
    // Enable laser of for measuring
    side.Laser(false);

    // ----- Front laser -----
    auto front = Sick_front();
    front.Laser(true);

    // ----- Test loop -----
    // Read inclination from accelerometer and distance from both sensors in loop and print it
    while(true){
        
        if(auto distance = front.Distance_avg()) {
             setprecision(1);
             cout << "{ \n \"depthPos\" :\n {\"x\" : " << accelerometer.Inclination() << ",\n \"y\" : " << *distance << "\n},\n \"rectangle\" : \n {\n \"h\" : 50.0, \n \"w\" : 60.0 \n}}                                                                                              " <<std::endl;
	     break;
        }
        else
        {
            distance=0.0;
        }
        

    }

    return 0;
}
