#include <drm_fourcc.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <unistd.h>

#include "bo.h"
#include "dev.h"
#include "drmDsp.h"
#include "modeset.h"

struct drmDsp {
  struct fb_var_screeninfo vinfo;
  unsigned long screensize;
  char* fbp;
  struct sp_dev* dev;
  struct sp_plane** plane;
  struct sp_crtc* test_crtc;
  struct sp_plane* test_plane;
  int num_test_planes;
  struct sp_bo* bo[2];
  struct sp_bo* nextbo;
} gDrmDsp;

int initDrmDsp() {
  int ret = 0, i = 0;
  struct drmDsp* pDrmDsp = &gDrmDsp;

  memset(pDrmDsp, 0, sizeof(struct drmDsp));

  pDrmDsp->dev = create_sp_dev();
  if (!pDrmDsp->dev) {
    fprintf(stderr, "Failed to create sp_dev\n");
    return -1;
  }

  ret = initialize_screens(pDrmDsp->dev);
  if (ret) {
    fprintf(stderr, "Failed to initialize screens\n");
    return ret;
  }
  pDrmDsp->plane = (struct sp_plane**)calloc(pDrmDsp->dev->num_planes,
                                             sizeof(struct sp_plane*));
  if (!pDrmDsp->plane) {
    fprintf(stderr, "Failed to allocate plane array\n");
    return -1;
  }

  pDrmDsp->test_crtc = &pDrmDsp->dev->crtcs[0];
  pDrmDsp->num_test_planes = pDrmDsp->test_crtc->num_planes;
  for (i = 0; i < pDrmDsp->test_crtc->num_planes; i++) {
    pDrmDsp->plane[i] = get_sp_plane(pDrmDsp->dev, pDrmDsp->test_crtc);
    if (is_supported_format(pDrmDsp->plane[i], DRM_FORMAT_NV12))
      pDrmDsp->test_plane = pDrmDsp->plane[i];
  }
  if (!pDrmDsp->test_plane) {
    fprintf(stderr, "pDrmDsp->test_plane is null\n");
    return -1;
  } else {
    fprintf(stderr, "pDrmDsp->test_plane is no null\n");
    return 0;
  }
}

void deInitDrmDsp() {
  struct drmDsp* pDrmDsp = &gDrmDsp;
  if (pDrmDsp->plane) free(pDrmDsp->plane);
  if (pDrmDsp->bo[0]) {
    drmModeRmFB(pDrmDsp->dev->fd, pDrmDsp->bo[0]->fb_id);
    free_sp_bo(pDrmDsp->bo[0]);
  }
  if (pDrmDsp->bo[1]) {
    drmModeRmFB(pDrmDsp->dev->fd, pDrmDsp->bo[1]->fb_id);
    free_sp_bo(pDrmDsp->bo[1]);
  }
  destroy_sp_dev(pDrmDsp->dev);
  memset(pDrmDsp, 0, sizeof(struct drmDsp));
}

static int arm_camera_yuv420_scale_arm(char* srcbuf, char* dstbuf, int src_w,
                                       int src_h, int dst_w, int dst_h) {
  unsigned char *psY, *pdY, *psUV, *pdUV;
  unsigned char *src, *dst;
  int srcW, srcH, cropW, cropH, dstW, dstH;
  long zoomindstxIntInv, zoomindstyIntInv;
  long x, y;
  long yCoeff00, yCoeff01, xCoeff00, xCoeff01;
  long sX, sY;
  long r0, r1, a, b, c, d;
  int ret = 0;
  int nv21DstFmt = 0, mirror = 0;
  int ratio = 0;
  int top_offset = 0, left_offset = 0;

  // need crop ?
  if ((src_w * 100 / src_h) != (dst_w * 100 / dst_h)) {
    ratio = ((src_w * 100 / dst_w) >= (src_h * 100 / dst_h))
                ? (src_h * 100 / dst_h)
                : (src_w * 100 / dst_w);
    cropW = ratio * dst_w / 100;
    cropH = ratio * dst_h / 100;

    left_offset = ((src_w - cropW) >> 1) & (~0x01);
    top_offset = ((src_h - cropH) >> 1) & (~0x01);
  } else {
    cropW = src_w;
    cropH = src_h;
    top_offset = 0;
    left_offset = 0;
  }

  src = psY = (unsigned char*)(srcbuf) + top_offset * src_w + left_offset;
  // psUV = psY +src_w*src_h+top_offset*src_w/2+left_offset;
  psUV = (unsigned char*)(srcbuf) + src_w * src_h + top_offset * src_w / 2 +
         left_offset;

  srcW = src_w;
  srcH = src_h;
  //	cropW = src_w;
  //	cropH = src_h;

  dst = pdY = (unsigned char*)dstbuf;
  pdUV = pdY + dst_w * dst_h;
  dstW = dst_w;
  dstH = dst_h;

  zoomindstxIntInv = ((unsigned long)(cropW) << 16) / dstW + 1;
  zoomindstyIntInv = ((unsigned long)(cropH) << 16) / dstH + 1;
  // y
  // for(y = 0; y<dstH - 1 ; y++ ) {
  for (y = 0; y < dstH; y++) {
    yCoeff00 = (y * zoomindstyIntInv) & 0xffff;
    yCoeff01 = 0xffff - yCoeff00;
    sY = (y * zoomindstyIntInv >> 16);
    sY = (sY >= srcH - 1) ? (srcH - 2) : sY;
    for (x = 0; x < dstW; x++) {
      xCoeff00 = (x * zoomindstxIntInv) & 0xffff;
      xCoeff01 = 0xffff - xCoeff00;
      sX = (x * zoomindstxIntInv >> 16);
      sX = (sX >= srcW - 1) ? (srcW - 2) : sX;
      a = psY[sY * srcW + sX];
      b = psY[sY * srcW + sX + 1];
      c = psY[(sY + 1) * srcW + sX];
      d = psY[(sY + 1) * srcW + sX + 1];

      r0 = (a * xCoeff01 + b * xCoeff00) >> 16;
      r1 = (c * xCoeff01 + d * xCoeff00) >> 16;
      r0 = (r0 * yCoeff01 + r1 * yCoeff00) >> 16;

      if (mirror)
        pdY[dstW - 1 - x] = r0;
      else
        pdY[x] = r0;
    }
    pdY += dstW;
  }

  dstW /= 2;
  dstH /= 2;
  srcW /= 2;
  srcH /= 2;

  // UV
  // for(y = 0; y<dstH - 1 ; y++ ) {
  for (y = 0; y < dstH; y++) {
    yCoeff00 = (y * zoomindstyIntInv) & 0xffff;
    yCoeff01 = 0xffff - yCoeff00;
    sY = (y * zoomindstyIntInv >> 16);
    sY = (sY >= srcH - 1) ? (srcH - 2) : sY;
    for (x = 0; x < dstW; x++) {
      xCoeff00 = (x * zoomindstxIntInv) & 0xffff;
      xCoeff01 = 0xffff - xCoeff00;
      sX = (x * zoomindstxIntInv >> 16);
      sX = (sX >= srcW - 1) ? (srcW - 2) : sX;
      // U
      a = psUV[(sY * srcW + sX) * 2];
      b = psUV[(sY * srcW + sX + 1) * 2];
      c = psUV[((sY + 1) * srcW + sX) * 2];
      d = psUV[((sY + 1) * srcW + sX + 1) * 2];

      r0 = (a * xCoeff01 + b * xCoeff00) >> 16;
      r1 = (c * xCoeff01 + d * xCoeff00) >> 16;
      r0 = (r0 * yCoeff01 + r1 * yCoeff00) >> 16;

      if (mirror && nv21DstFmt)
        pdUV[dstW * 2 - 1 - (x * 2)] = r0;
      else if (mirror)
        pdUV[dstW * 2 - 1 - (x * 2 + 1)] = r0;
      else if (nv21DstFmt)
        pdUV[x * 2 + 1] = r0;
      else
        pdUV[x * 2] = r0;
      // V
      a = psUV[(sY * srcW + sX) * 2 + 1];
      b = psUV[(sY * srcW + sX + 1) * 2 + 1];
      c = psUV[((sY + 1) * srcW + sX) * 2 + 1];
      d = psUV[((sY + 1) * srcW + sX + 1) * 2 + 1];

      r0 = (a * xCoeff01 + b * xCoeff00) >> 16;
      r1 = (c * xCoeff01 + d * xCoeff00) >> 16;
      r0 = (r0 * yCoeff01 + r1 * yCoeff00) >> 16;

      if (mirror && nv21DstFmt)
        pdUV[dstW * 2 - 1 - (x * 2 + 1)] = r0;
      else if (mirror)
        pdUV[dstW * 2 - 1 - (x * 2)] = r0;
      else if (nv21DstFmt)
        pdUV[x * 2] = r0;
      else
        pdUV[x * 2 + 1] = r0;
    }
    pdUV += dstW * 2;
  }
  return 0;
}

int drmDspFrame(int width, int height, void* dmaFd, int fmt) {
  int ret;
  struct drm_mode_create_dumb cd;
  struct sp_bo* bo;
  struct drmDsp* pDrmDsp = &gDrmDsp;

  int ori_width = width;
  int ori_height = height;

  if (width > 1920) {
    width = 1920;
    height = 1080;
  }

  int wAlign16 = ((width + 15) & (~15));
  int hAlign16 = ((height + 15) & (~15));
  int frameSize = wAlign16 * hAlign16 * 3 / 2;
  uint32_t handles[4], pitches[4], offsets[4];

  if (DRM_FORMAT_NV12 != fmt) {
    fprintf(stderr, "%s just support NV12 to display\n", __func__);
    return -1;
  }
// create bo
#if 1
  if (!pDrmDsp->bo[0]) {
    fprintf(stderr, "%s:bo widthxheight:%dx%d\n", __func__, wAlign16, hAlign16);
    pDrmDsp->bo[0] = create_sp_bo(pDrmDsp->dev, wAlign16, hAlign16, 16, 32,
                                  DRM_FORMAT_NV12, 0);
    pDrmDsp->bo[1] = create_sp_bo(pDrmDsp->dev, wAlign16, hAlign16, 16, 32,
                                  DRM_FORMAT_NV12, 0);
    if (!pDrmDsp->bo[0] || !pDrmDsp->bo[1]) {
      fprintf(stderr, "%s:create bo failed ! \n", __func__);
      return -1;
    }
    pDrmDsp->nextbo = pDrmDsp->bo[0];
  }

  if (!pDrmDsp->nextbo) {
    fprintf(stderr, "%s:no available bo ! \n", __func__);
    return -1;
  }

  bo = pDrmDsp->nextbo;
#else
  bo = create_sp_bo(pDrmDsp->dev, wAlign16, hAlign16, 16, 32, DRM_FORMAT_NV12,
                    0);
  if (!bo) fprintf(stderr, "%s:create bo failed ! \n", __func__);
#endif

  handles[0] = bo->handle;
  pitches[0] = wAlign16;
  offsets[0] = 0;
  handles[1] = bo->handle;
  pitches[1] = wAlign16;
  offsets[1] = width * height;  // wAlign16 * hAlign16;
  // copy src data to bo
  if (ori_width == width)
    memcpy(bo->map_addr, dmaFd, wAlign16 * hAlign16 * 3 / 2);
  else
    arm_camera_yuv420_scale_arm(dmaFd, bo->map_addr, ori_width, ori_height,
                                width, height);
  if (bo->fb_id <= 0) {
    ret = drmModeAddFB2(bo->dev->fd, bo->width, bo->height, bo->format, handles,
                        pitches, offsets, &bo->fb_id, bo->flags);
    if (ret) {
      fprintf(stderr, "%s:failed to create fb ret=%d\n", __func__, ret);
      fprintf(stderr,
          "fd:%d "
          ",wxh:%ux%u,format:%u,handles:%u,%u,pictches:%u,%u,offsets:%u,%u,fb_id:"
          "%u,flags:%u \n",
          bo->dev->fd, bo->width, bo->height, bo->format, handles[0], handles[1],
          pitches[0], pitches[1], offsets[0], offsets[1], bo->fb_id, bo->flags);
      return ret;
    } else {
      fprintf(stderr, "%s:create fb success fb_id:%u\n", __func__, bo->fb_id);
    }
  }

  ret = drmModeSetPlane(pDrmDsp->dev->fd, pDrmDsp->test_plane->plane->plane_id,
                        pDrmDsp->test_crtc->crtc->crtc_id, bo->fb_id, 0, 0, 0,
                        // pDrmDsp->test_crtc->crtc->mode.hdisplay,
                        wAlign16, hAlign16,
                        // pDrmDsp->test_crtc->crtc->mode.vdisplay,
                        0, 0, width << 16, height << 16);
  if (ret) {
    fprintf(stderr, "failed to set plane to crtc ret=%d\n", ret);
    return ret;
  }
// free_sp_bo(bo);
#if 0
  if (pDrmDsp->test_plane->bo) {
    if (pDrmDsp->test_plane->bo->fb_id) {
      ret = drmModeRmFB(pDrmDsp->dev->fd, pDrmDsp->test_plane->bo->fb_id);
      if (ret)
        fprintf(stderr, "Failed to rmfb ret=%d!\n", ret);
    }
    if (pDrmDsp->test_plane->bo->handle) {
      struct drm_gem_close req = {
        .handle = pDrmDsp->test_plane->bo->handle,
      };

      drmIoctl(bo->dev->fd, DRM_IOCTL_GEM_CLOSE, &req);
      fprintf(stderr, "%s:close bo success!\n", __func__);
    }

    if (!pDrmDsp->nextbo)
      free_sp_bo(pDrmDsp->test_plane->bo);
  }
  pDrmDsp->test_plane->bo = bo; //last po
#endif
#if 1
  // switch bo
  if (pDrmDsp->nextbo == pDrmDsp->bo[0])
    pDrmDsp->nextbo = pDrmDsp->bo[1];
  else
    pDrmDsp->nextbo = pDrmDsp->bo[0];
#endif
}