#include <iostream>

#include <k230_sensor.h>
#include <k230_venc.h>

Sensor *sensor = nullptr;
VideoEncoder *venc = nullptr;

void setup() {
  // put your setup code here, to run once:
  int width = 1920;
  int height = 1080;

  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(width, height,VICAP_CHN_ID_0)) {
    std::cout << "Failed to set sensor framesize" << std::endl;
    return;
  }

  if(0x00 != sensor->set_channel_alignment(12,VICAP_CHN_ID_0)){
    std::cout << "Failed to set sensor alignment" << 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;
  }

  EncoderConfig config(VENC_H264,VENC_PROFILE_H264_MAIN,width,height);
  venc = new VideoEncoder(config);

  if(0x00 != venc->create()){
    std::cout << "Failed to create venc channel" << std::endl;
    return;
  }

  if (0x00 != venc->start()){
    std::cout << "Failed to start venc channel" << std::endl;
    return;
  }

}

void loop() {
  k_video_frame_info vf_info;
  EncodedStream stream;
  static int stream_cnt = 0;

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

      if (0 == venc->send_frame(&vf_info,10)){
          if (0 == venc->get_stream(stream)){
              for (int i =0;i < stream.packet_count;i ++){
                stream_cnt ++;
                printf("[%d]get stream size:%d,type:%d\n",stream_cnt,stream.packets[i].size,stream.packets[i].type);
              }

              venc->release_stream(stream);
          }
      }

  }
}
