#include <iostream>

#include <k230_sensor.h>

Sensor *sensor = nullptr;

void setup() {
  // put your setup code here, to run once:

  sensor = new Sensor(2);
  if(!sensor) {
    std::cout << "Too many sensors" << std::endl;
    return;
  }

  if(0x00 != sensor->reset()) {
    std::cout << "Failed to probe sensor" << std::endl;
    return;
  }

  if(0x00 != sensor->set_framesize(1920, 1080)) {
    std::cout << "Failed to set sensor framesize" << std::endl;
    return;
  }

  if(0x00 != sensor->set_pixformat(SensorPixelFormat::YUV420SP)) {
    std::cout << "Failed to set sensor pixelformat" << std::endl;
    return;
  }

  if(0x00 != sensor->run()) {
    std::cout << "Failed run sensor" << std::endl;
    return;
  }
}

void loop() {
  k_video_frame_info vf_info;

  if(sensor) {
    if(0x00 != sensor->snapshot(vf_info)) {
    std::cout << "Failed snapshot sensor" << std::endl;
    return;
    }
    std::cout << "Snapshot done." << std::endl;
  }
}
