/** Copyright (C) 2010 Texas Instruments, Inc. All rights reserved */

/** This is a simple, dumbed down version of the vcam_test which only uses
    command line paramters and simple settings in order to test out system
    connectivity, this is *not* intended to replace the more complicated
    VisionCamTest.cpp. Use export VCAM_SIMPLE=1 to use this version.
*/

#include <limits.h>

#include <autolock.h>
#include <output.h>

#include <OMXVisionCam.h>

/** The default number of display buffers */
#define DISPLAY_NUM_BUFFERS (5)

#define STATUS_FAILED(x)    (x < 0)

#define CAMERA_NAME MODULE_NAME("vcam")

typedef enum _option_type_e {
    OPTION_TYPE_BOOL,
    OPTION_TYPE_INT,
    OPTION_TYPE_HEX,
    OPTION_TYPE_FLOAT,
    OPTION_TYPE_STRING,
} option_type_e;

typedef struct _option_t {
    option_type_e type;
    void *datum;
    size_t size;
    const char *short_switch;
    const char *long_switch;
    const char *description;
} option_t;

size_t option_process(int argc, char *argv[], option_t *opts, size_t numOpts)
{
    int i;
    size_t j,c = 0;
    for (i = 0; i < argc; i++)
    {
        for (j = 0; j < numOpts; j++)
        {
            if ((opts[j].short_switch && strcmp(opts[j].short_switch, argv[i]) == 0) ||
                (opts[j].long_switch && strcmp(opts[j].long_switch, argv[i]) == 0))
            {
                switch (opts[j].type)
                {
                    case OPTION_TYPE_BOOL:
                        *(bool *)opts[j].datum = true;
                        c++;
                        break;
                    case OPTION_TYPE_INT:
                        if (i+1 < argc && opts[j].size == sizeof(int)) {
                            i += 1;
                            sscanf(argv[i],"%d",(int *)opts[j].datum);
                            c++;
                        }
                        break;
                    case OPTION_TYPE_HEX:
                        if (i+1 < argc && opts[j].size == sizeof(uint32_t)) {
                            i += 1;
                            sscanf(argv[i],"%x",(uint32_t *)opts[j].datum);
                            c++;
                        }
                        break;
                    case OPTION_TYPE_FLOAT:
                        if (i+1 < argc && opts[j].size == sizeof(float)) {
                            i += 1;
                            sscanf(argv[i],"%f",(float *)opts[j].datum);
                            c++;
                        }
                        break;
                    case OPTION_TYPE_STRING:
                        if (i+1 < argc) {
                            i += 1;
                            strncpy(static_cast<char*>(opts[j].datum), argv[i], opts[j].size);
                            c++;
                        }
                        break;
                }
                break; // process next argv
            }
        }
    }
    return c;
}

int32_t display_width;
int32_t display_height;
int32_t width;
int32_t height;
int32_t fps;
uint32_t color;
char fourcc_str[5];
int32_t numImages;
int32_t recvFrames;
int32_t numFrames;
int32_t sensor;
int32_t frameLock;
int32_t focusDepth;
int32_t mode;
int32_t camera_rotation;
uint32_t screen_rotation;
uint32_t dw,sw;
uint32_t dh,sh;
int32_t subsample;
bool twoD;
bool topBottom;
bool file_ouput;
char name[PATH_MAX];
uint32_t numTimeouts;
uint32_t white;
uint32_t brightness;
uint32_t iso;
uint32_t exposure;
bool manual;
uint32_t repeats;
option_t opts[] = {
    {OPTION_TYPE_INT, &display_width, sizeof(display_width), "-dw",  "--display_width", "Width of display"},
    {OPTION_TYPE_INT, &display_height, sizeof(display_height), "-dh",  "--display_height", "Height of display"},
    {OPTION_TYPE_INT, &width, sizeof(width),              "-w",  "--width", "Width of image"},
    {OPTION_TYPE_INT, &height, sizeof(height),            "-h",  "--height", "Height of image"},
    {OPTION_TYPE_INT, &fps, sizeof(fps),                  "-f",  "--fps", "Frame Rate"},
    {OPTION_TYPE_BOOL, &twoD, sizeof(twoD),               "-2d", "--two-d", "Use 2D buffers"},
    {OPTION_TYPE_BOOL, &topBottom, sizeof(topBottom),     "-tb", "--topbottom", "Orient the stereo image as top/bottom"},
    {OPTION_TYPE_STRING, &fourcc_str, sizeof(fourcc_str), "-c",  "--fourcc", "FOURCC Code as string (UYVY) "},
    {OPTION_TYPE_STRING, name, sizeof(name),              "-n",  "--name", "Name of file to read"},
    {OPTION_TYPE_INT, &numImages, sizeof(numImages),      "-i",  "--images", "Number of images to use"},
    {OPTION_TYPE_INT, &numFrames, sizeof(numFrames),      "-#",  "--frames", "Number of frames to process"},
    {OPTION_TYPE_INT, &sensor, sizeof(sensor),            "-s",  "--sensor", "Selects the sensor (0,1,2)"},
    {OPTION_TYPE_INT, &frameLock, sizeof(frameLock),      "-l",  "--lock-after", "Locks AE/AWB after specified frame count"},
    {OPTION_TYPE_INT, &focusDepth, sizeof(focusDepth),    "-fd", "--focus-depth", "Specific Focus Depth [0-150]"},
    {OPTION_TYPE_INT, &mode, sizeof(mode),                "-p",  "--mode", "Capture Mode"},
    {OPTION_TYPE_INT, &camera_rotation, sizeof(camera_rotation),"-cr", "--camera_rotation", "Rotates the captured image in the camera"},
    {OPTION_TYPE_INT, &screen_rotation, sizeof(screen_rotation),"-sr", "--screen_rotation", "Rotates the display image"},
    {OPTION_TYPE_INT, &subsample, sizeof(subsample),      "-sb", "--subsample", "Subsampled ratio, defaults to 1"},
    {OPTION_TYPE_BOOL, &file_ouput, sizeof(file_ouput),   "-o",  "--out", "Write to file instead of display"},
    {OPTION_TYPE_BOOL, &manual, sizeof(manual),           "-m",  "--manual", "Use manual settings"},
    {OPTION_TYPE_INT, &white, sizeof(white),              "-wb", "--white", "White Balance Mode"},
    {OPTION_TYPE_INT, &brightness, sizeof(brightness),    "-br", "--bright", "Brightness Value"},
    {OPTION_TYPE_INT, &iso, sizeof(iso),                  "-is", "--iso", "ISO Value"},
    {OPTION_TYPE_INT, &exposure, sizeof(exposure),        "-ex", "--exposure", "Manual Exposure Value"},
    {OPTION_TYPE_INT, &numTimeouts, sizeof(numTimeouts),  "-to", "--timeouts", "Set the number of frame timeout which can occur before the camera halts"},
    {OPTION_TYPE_INT, &repeats, sizeof(repeats),          "-r",  "--repeat", "Sets the number of repeat iterations, default is 1."},
};

struct CallbackData {
  pthread_mutex_t            mutex;
  std::list<VisionCamFrame*> frames;
  CallbackData() {
    pthread_mutex_init(&mutex, NULL);
  }
  ~CallbackData() {
    pthread_mutex_destroy(&mutex);
  }
};

void VisionCamTestCallback(VisionCamFrame* frame) {
  CallbackData* cb_data = static_cast<CallbackData*>(frame->mCookie);

  MSG("New frame received; offset=%ux%u", frame->mOffsetX, frame->mOffsetY);
  AutoLock(&cb_data->mutex);
  cb_data->frames.push_back(frame);
}

int main(int argc, char *argv[]) {
  uint32_t r = 0;
  int32_t i = 0;
  VisionCamSensorSelection sensorIndex = VCAM_SENSOR_SECONDARY;
  VisionCamCaptureMode capmode = VCAM_VIDEO_NORMAL;
  VisionCamFlickerType flicker = FLICKER_60Hz;
  VisionCamFocusMode focus = VCAM_FOCUS_CONTROL_AUTO;
  VisionCamStereoInfo info;

  // default values
  white = VCAM_WHITE_BAL_CONTROL_AUTO;
  brightness = 50; // [0-200]
  iso = 100;       // [100-1600]
  exposure = 50;   // [0-100]
  manual = false;
  recvFrames = 0;
  display_width = 640;
  display_height = 480;
  width = 640;
  height = 480;
  fps = 30;
  strcpy(fourcc_str, "NV12");
  color = FOURCC_STR(fourcc_str);
  strcpy(name, "car");
  numImages = DISPLAY_NUM_BUFFERS;
  sensor = OMX_TI_StereoSensor;
  numFrames = 100;
  frameLock = 0xFFFFFFFF;
  focusDepth = -1;
  mode = VCAM_VIDEO_NORMAL;
  camera_rotation = 0;
  screen_rotation = 0;
  dw = 0;
  dh = 0;
  sw = 0;
  sh = 0;
  twoD = false;
  topBottom = false;
  subsample = 1;
  file_ouput = false;
  numTimeouts = 10;
  repeats = 1;

  option_process(argc, argv, opts, sizeof(opts)/sizeof(opts[0]));

  // check for bad input
  if (width <= 0) width = 160;
  if (height <= 0) height = 120;
  if (sw <= 0) sw = width;
  if (sh <= 0) sh = height;
  if (numImages <= 2) numImages = 2;
  if (fps <= 15) fps = 15;
  if (numFrames <= 10) numFrames = 10;
  if (frameLock > numFrames) frameLock = -1;
  if (focusDepth > 150) focusDepth = 75;
  if (mode >= VCAM_CAP_MODE_MAX) mode = VCAM_GESTURE_MODE;
  if (sensor > 2) sensor = 2;
  if (camera_rotation != 0 && camera_rotation != 90 && camera_rotation != 180 && camera_rotation != 270)
      camera_rotation = 0;
  else if (camera_rotation == 90 || camera_rotation == 270)
  {
      uint32_t t = sw;
      sw = sh;
      sh = t;
  }
  if (subsample <= 0 || subsample > 4)
      subsample = 1;
  if (brightness > 200)
      brightness = 200;
  if (iso < 100) iso = 100;
  if (iso > 1600) iso = 1600;
  if (exposure > 100) exposure = 100;
  if (repeats == 0)
      repeats = 1;

  color = FOURCC_STR(fourcc_str);

  MSG("Requested Color %08x", color);

  switch (sensor)
  {
    case 2:
      sensorIndex = VCAM_SENSOR_STEREO;
      mode = VCAM_STEREO_MODE;
      memset(&info, 0, sizeof(info));
      if (topBottom) {
        MSG("Enabling Stereo Use Case (top/bottom)!");
        info.layout = VCAM_STEREO_LAYOUT_TOPBOTTOM;
        height *= 2;
      } else {
        MSG("Enabling Stereo Use Case (side by side)!");
        info.layout = VCAM_STEREO_LAYOUT_LEFTRIGHT;
        width *= 2;
      }
      info.subsampling = subsample;
      if (color != FOURCC('N','V','1','2')) {
        ERROR("Only NV12 is supported for stereo");
        exit(1);
      }
      break;
    case 1:
      MSG("Enabling Vision Mode on Front Camera");
      sensorIndex = VCAM_SENSOR_SECONDARY;
      mode = VCAM_VIDEO_NORMAL;
      break;
    default:
      MSG("Enabling Vision Mode on Back Camera!");
      sensorIndex = VCAM_SENSOR_PRIMARY;
  }
  capmode = (VisionCamCaptureMode)mode;

  // using 1 to <= so prints will make sense
  for (r = 1; r <= repeats; r++)
  {
    MSG("Iteration %u of %u", r, repeats);
    recvFrames = 0;
    OMXVisionCam omx_cam;
    VisionCam *pCam = &omx_cam;
    int greError = 0;

    CallbackData cb_data;
    VCAM_RETURN_IF_FAILED(greError, pCam->init(&cb_data));
    VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_WIDTH, &width, sizeof(width)));
    VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_HEIGHT, &height, sizeof(height)));
    VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_COLOR_SPACE_FOURCC, &color, sizeof(color)));
    // Can't set ROTATION here, see below
    VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_FPS_FIXED, &fps, sizeof(fps)));
    VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_CAP_MODE, &capmode, sizeof(capmode)));
    if ((sensorIndex >= 0) && (sensorIndex <= 1))
    {
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_SENSOR_SELECT, &sensorIndex, sizeof(sensorIndex)));
    }
    if (capmode == VCAM_STEREO_MODE)
    {
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_STEREO_INFO, &info, sizeof(info)));
    }

    // VCAM_PARAM_2DBUFFER_DIM should only be called after resolutions, color space, cap mode, and
    //   optionally stereo information is set.
    VisionCamResType res = { VCAM_RESOL_MAX, width, height };
    VCAM_COMPLAIN_IF_FAILED(greError, pCam->getParameter(VCAM_PARAM_2DBUFFER_DIM, &res, sizeof(res)));
    MSG("Image: %ux%u, buffer: %ux%u", width, height, res.mWidth, res.mHeight);

    Output* output = new Output(display_width, display_height, width, height,
        res.mWidth, res.mHeight, color, numImages, twoD, file_ouput);
    if (output->open() == 0)
    {
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_FLICKER, &flicker, sizeof(flicker)));
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_AWB_MODE, &white, sizeof(white)));
        if (manual)
        {
            VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_BRIGHTNESS, &brightness, sizeof(brightness)));
            VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_EXPOSURE_ISO, &iso, sizeof(iso)));
            VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_MANUAL_EXPOSURE, &exposure, sizeof(exposure)));
        }
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_NAME, name, sizeof(name)));

        struct buffer* images[numImages];
        BufferMeta meta[numImages];
        for (i = 0; i < numImages; i++)
        {
            images[i] = output->alloc(&meta[i]);
        }
        // tell the camera to use all the camera index buffers
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->useBuffers(meta, numImages));

        /** @todo Since the OMX Camera is probably being used by
            the VCAM_SIMPLE test, we have to understand that
            the OMX-CAMERA has a bug in the camera_rotation when used
            during LOADED state. We have to wait until IDLE
            (post useBuffers) or EXECUTING (post PREVIEW)
            before rotating. */
        /** @todo Additionally, OMX-CAMERA STEREO mode can't handle the rotation values! */
        if (capmode != VCAM_STEREO_MODE)
            VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_ROTATION, &camera_rotation, sizeof(camera_rotation)));

        // register the engine callback with the camera
        VCAM_COMPLAIN_IF_FAILED(greError,  pCam->enablePreviewCbk(VisionCamTestCallback));
        VCAM_COMPLAIN_IF_FAILED(greError,  pCam->sendCommand(VCAM_CMD_PREVIEW_START));

        if (focusDepth == -1) { // begin auto-focus
            VCAM_COMPLAIN_IF_FAILED(greError,  pCam->setParameter(VCAM_PARAM_DO_AUTOFOCUS, &focus, sizeof(focus)));
        }

        if (greError == 0)
        {
            uint32_t timeouts = 0;
            usleep(1000000/fps); // wait 1 frame period.
            MSG("VisionCam is initialized, entering queue read loop!");
            // read from the queue and display the images
            do {
              if (cb_data.frames.empty()) {
                ERROR("Timedout waiting for buffer from Camera!");
                timeouts++;
                usleep(1000000/fps);
                continue;
              }
              VisionCamFrame* frame;
              {
                AutoLock(&cb_data.mutex);
                frame = cb_data.frames.front();
                cb_data.frames.pop_front();
              }
              if (frame != NULL) {
                BufferMeta* meta = static_cast<BufferMeta*>(frame->mFrameBuff);
                timeouts = 0;
                output->render(meta);
                pCam->returnFrame(frame);
                recvFrames++;
                if (recvFrames >= numFrames) {
                  break;
                }
                if (focusDepth >= 0) {
                  if (recvFrames == fps) { // after 1 second
                      VCAM_COMPLAIN_IF_FAILED(greError, pCam->setParameter(VCAM_PARAM_DO_MANUALFOCUS, &focusDepth, sizeof(focusDepth)));
                  }
                }
                if (frameLock > 0) {
                  if (recvFrames == frameLock) {
                      bool lock = true;
                      VCAM_COMPLAIN_IF_FAILED(greError, pCam->sendCommand(VCAM_CMD_LOCK_AE, &lock, sizeof(lock)));
                      VCAM_COMPLAIN_IF_FAILED(greError, pCam->sendCommand(VCAM_CMD_LOCK_AWB, &lock, sizeof(lock)));
                  }
                }
              }
            } while (timeouts < numTimeouts);
        }
        else
        {
            ERROR("VCAM_TEST Failed during initialization (greError = %d, 0x%08x)!", greError, greError);
        }

        // destroy the camera
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->sendCommand(VCAM_CMD_PREVIEW_STOP));
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->disablePreviewCbk(VisionCamTestCallback));
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->releaseBuffers());
        VCAM_COMPLAIN_IF_FAILED(greError, pCam->deinit());

        // free the images
        for (i = 0; i < numImages; i++){
            output->free(&meta[i]);
        }

        output->close();
        delete output;
    }
  }
  return 0;
}