#include <stdint.h>

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include <linux/media.h>
#include <linux/videodev2.h>
#include <linux/v4l2-subdev.h>

#include <gst/gst.h>
#include "pinesrcbin.h"
#include "utility.h"

static const struct media_v2_link *
gst_pinesrcbin_get_link_from_entity(struct device_data *dev, uint32_t id);
static const struct media_v2_interface *
gst_pinesrcbin_get_interface_by_id(struct device_data *dev, uint32_t id);
static const struct media_v2_interface *
gst_pinesrcbin_get_entity_interface(struct device_data *dev, uint32_t id);
static const struct media_v2_pad *
gst_pinesrcbin_get_pad_from_entity(struct device_data *dev, uint32_t id);
static const struct media_v2_entity *
gst_pinesrcbin_find_media_entity(struct device_data *dev,
                                 const char *driver_name);
static struct device_data *
gst_pinesrcbin_find_media_device(GstPineSrcBin *self, const char *driver_name);
static gboolean
get_device_path(struct media_v2_intf_devnode devnode, char *path, int length);
static gboolean
query_autofocus(int fd);
static void
setup_gain(int fd, int *gain_ctrl, int *gain_max);
static gboolean
camera_control(int fd, uint32_t id, int request, uint32_t *value);
static inline const struct media_v2_pad *
get_device_pad(const struct device_data *dev, uint32_t id);
static gboolean
setup_link(const struct device_data *dev, uint32_t source_pad_id,
           uint32_t sink_pad_id, gboolean enabled);

static int alloc_device(struct device_data *dev);
static void disable_all_links(struct device_data *dev);
static void subdev_set_mode(int subdev_fd, int width,
		int height,
		uint32_t bus_code,
		struct v4l2_fract interval);

struct camera_config;
int get_camera_config_no(const char *conf_name);
static struct camera_config *
get_camera_config(const char *conf_name);

struct camera_data;
static struct camera_data *
get_camera_data(const char *conf_name);

static int
xioctl (int fd, int request, void *arg)
{
  int r;
  do {
    r = ioctl (fd, request, arg);
  } while (r == -1 && errno == EINTR);
  return r;
}

struct camera_config {
    char conf_name[128];
    char camera_name[128];
    int width;
    int height;
    uint32_t bus_code_video;
    uint32_t bus_code_single;
    struct v4l2_fract frame_interval;
};

struct camera_data {
	struct camera_config *conf;
	int subdev_fd;
	const struct media_v2_entity *entity;
	const struct media_v2_interface *interface;
	int gain_ctrl;
	int gain_max;
	char devnode_path[261];
};
struct camera_config configurations[] = {
	{"front", "gc2145", 320, 240, MEDIA_BUS_FMT_YUYV8_2X8, MEDIA_BUS_FMT_SBGGR8_1X8, {1, 15}},
	{"rear", "ov5640", 320, 240, MEDIA_BUS_FMT_YUYV8_2X8, MEDIA_BUS_FMT_SBGGR8_1X8, {1, 15}},
};
struct camera_data cam_store[] = {
	{},
	{}
};

struct mediabus_to_gst {
	uint32_t bus_code;
	const char *gst_format;
};
static struct mediabus_to_gst gst_format[] = {
	{MEDIA_BUS_FMT_SBGGR8_1X8, "BGGR"},
	{MEDIA_BUS_FMT_SGBRG8_1X8, "GBRG"},
	{MEDIA_BUS_FMT_SGRBG8_1X8, "GRBG"},
	{MEDIA_BUS_FMT_SRGGB8_1X8, "RGGB"},
	{MEDIA_BUS_FMT_SBGGR10_1X10, "BGGR10"},
	{MEDIA_BUS_FMT_SGBRG10_1X10, "GBRG10"},
	{MEDIA_BUS_FMT_SGRBG10_1X10, "GRBG10"},
	{MEDIA_BUS_FMT_SRGGB10_1X10, "RGGB10"},
	{MEDIA_BUS_FMT_UYVY8_2X8, "UYVY"},
	{MEDIA_BUS_FMT_YUYV8_2X8, "YUY2"},
};
void gst_pinesrcbin_get_gst_caps_data(const char * conf_name,
		int *width, int * height,
		const char **format,
		struct v4l2_fract *interval)
{
	int i;
	const struct camera_config *conf = get_camera_config(conf_name);
	*width = conf->width;
	*height = conf->height;
	*format = NULL;
	*interval = conf->frame_interval;
	for (i = 0; i < sizeof(gst_format)/sizeof(gst_format[0]); i++) {
		if (conf->bus_code_video == gst_format[i].bus_code) {
			*format = gst_format[i].gst_format;
			return;
		}
	}
	g_assert(*format);
}

static const struct media_v2_link *
gst_pinesrcbin_get_link_from_entity(struct device_data *dev, uint32_t id)
{
    int i;
    for (i = 0; i < dev->num_links; i++)
        if (dev->links[i].sink_id == id)
            return &dev->links[i];
    return NULL;
}

static const struct media_v2_interface *
gst_pinesrcbin_get_interface_by_id(struct device_data *dev, uint32_t id)
{
    int i;
    for (i = 0; i < dev->num_interfaces; i++)
        if (dev->interfaces[i].id == id)
            return &dev->interfaces[i];
    return NULL;
}

static const struct media_v2_interface *
gst_pinesrcbin_get_entity_interface(struct device_data *dev, uint32_t id)
{
    const struct media_v2_link *link =
        gst_pinesrcbin_get_link_from_entity(dev, id);
    g_print("entity id: %d\n", id);
    if (!link)
        return NULL;
    g_print("link id: %d %d\n", link->sink_id, link->source_id);
    return gst_pinesrcbin_get_interface_by_id(dev, link->source_id);
}

static const struct media_v2_pad *
gst_pinesrcbin_get_pad_from_entity(struct device_data *dev, uint32_t id)
{
    int i;
    for (i = 0; i < dev->num_pads; i++)
    {
        if (dev->pads[i].entity_id == id)
            return &dev->pads[i];
    }
    return NULL;
}

static const struct media_v2_entity *
gst_pinesrcbin_find_media_entity(struct device_data *dev,
                                 const char *driver_name)
{
    int length = strlen(driver_name);
    int i;
    for (i = 0; i < dev->num_entities; i++)
        g_print("%s: entity: %s:%d:%s\n", dev->path, dev->entities[i].name,
                dev->entities[i].id, driver_name);
    for (i = 0; i < dev->num_entities; i++) {
        if (!strncmp(dev->entities[i].name, driver_name, length))
            return &dev->entities[i];
    }
    return NULL;
}

static void disable_all_links(struct device_data *dev)
{
    int i, j;
    for (i = 0; i < dev->num_entities; i++) {
	    const struct media_v2_pad *sink_pad = 
	    gst_pinesrcbin_get_pad_from_entity (dev, dev->entities[i].id);
	    if (!sink_pad)
		    continue;
	    if (!(sink_pad->flags & MEDIA_PAD_FL_SINK))
		    continue;
	    g_print("sink pad: %d\n", sink_pad->id);
    	    for (j = 0; j < dev->num_entities; j++) {
		    if (i == j)
			    continue;
	    	const struct media_v2_pad *source_pad = 
	    	gst_pinesrcbin_get_pad_from_entity (dev, dev->entities[j].id);
		if (!source_pad)
			continue;
	    	g_print("source pad: %d\n", source_pad->id);
	    	if (!(source_pad->flags & MEDIA_PAD_FL_SOURCE))
			    continue;
		g_print("disabling link %d %d\n", source_pad->entity_id, sink_pad->entity_id);
                setup_link(dev, source_pad->id, sink_pad->id, FALSE);
	    }
    }
#if 0
      const struct media_v2_link *plink =
	      gst_pinesrcbin_get_link_from_entity(dev,
		      dev->entities[i].id);
      if (plink) {
              g_print("link %d %08x %08x: %08x\n", plink->id, plink->source_id, plink->sink_id, plink->flags);
	      if (plink->flags & MEDIA_LNK_FL_ENABLED) {
		    uint32_t link_type = plink->flags & MEDIA_LNK_FL_LINK_TYPE;
		    if (link_type == MEDIA_LNK_FL_DATA_LINK) {
			    g_print("data link %d\n", plink->id);
			    setup_link(dev, plink->source_id, plink->sink_id, FALSE);
		    } else if (link_type == MEDIA_LNK_FL_INTERFACE_LINK) {
			    struct media_v2_interface *iface = st_pinesrcbin_get_interface_by_id(dev, plink->source_id);
			    g_print("interface link %d entity: %d\n", plink->id, iface->);
			    continue;
		    }
		    const struct media_v2_pad *source_pad, *sink_pad;
		    g_print("link %d is active\n", plink->id);
		    source_pad = gst_pinesrcbin_get_pad_from_entity(dev, plink->source_id);
		    sink_pad = gst_pinesrcbin_get_pad_from_entity(dev, plink->sink_id);
		    g_assert(source_pad);
		    g_assert(sink_pad);
		    setup_link(dev, source_pad->id, sink_pad->id, FALSE);
#if 0
		    struct media_link_desc link = {};
		    link.flags = 0;
    		    link.source.entity = plink->source_id;
    		    link.source.index = 0;
                    link.sink.entity = plink->sink_id;
    		    link.sink.index = 0;
    		    if (xioctl(dev->fd, MEDIA_IOC_SETUP_LINK, &link) == -1)
        		g_printerr("MEDIA_IOC_SETUP_LINK: %d %s", errno, strerror(errno));
#endif
#if 0
		    /* TODO: maintain own links structure */
		    else
			plink->flags = 0;
#endif
      		}
      }
    }
#endif
}

static struct device_data *
gst_pinesrcbin_find_media_device(GstPineSrcBin *self, const char *driver_name)
{
    int length = strlen(driver_name);
    GList *l = self->media_list;
    while (l)
    {
        struct device_data *dev = l->data;
        const struct media_device_info *info = &dev->info;
        g_print("device %s:%s:%s\n", dev->path, info->driver, driver_name);
        if (!strncmp(info->driver, driver_name, length))
            return dev;
        l = l->next;
    }
    return NULL;
}

static gboolean
get_device_path(struct media_v2_intf_devnode devnode, char *path, int length)
{
    char uevent_path[256];
    snprintf(uevent_path, 256, "/sys/dev/char/%d:%d/uevent", devnode.major,
             devnode.minor);

    FILE *f = fopen(uevent_path, "r");
    if (!f)
    {
        return FALSE;
    }

    char line[512];
    while (fgets(line, 512, f))
    {
        if (strncmp(line, "DEVNAME=", 8) == 0)
        {
            // Drop newline
            int length = strlen(line);
            if (line[length - 1] == '\n')
                line[length - 1] = '\0';

            snprintf(path, length, "/dev/%s", line + 8);
            fclose(f);
            return TRUE;
        }
    }

    fclose(f);

    return FALSE;
}

static gboolean
query_autofocus(int fd)
{
    struct v4l2_query_ext_ctrl ctrl = {};
    ctrl.id = V4L2_CID_FOCUS_AUTO;
    int rc = xioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
    if (rc < 0)
        return FALSE;
    return TRUE;
}

static void
setup_gain(int fd, int *gain_ctrl, int *gain_max)
{
    struct v4l2_query_ext_ctrl ctrl = {};
    ctrl.id = V4L2_CID_GAIN;
    int rc = xioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
    if (rc >= 0)
    {
        *gain_ctrl = ctrl.id;
        *gain_max = ctrl.maximum;
        return;
    }
    ctrl.id = V4L2_CID_ANALOGUE_GAIN;
    rc = xioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
    if (rc >= 0)
    {
        *gain_ctrl = ctrl.id;
        *gain_max = ctrl.maximum;
        return;
    }
}

static gboolean
camera_control(int fd, uint32_t id, int request, uint32_t *value)
{
    struct v4l2_ext_control ctrl = {};
    ctrl.id = id;
    ctrl.value = *value;

    struct v4l2_ext_controls ctrls = {
        .ctrl_class = 0,
        .which = V4L2_CTRL_WHICH_CUR_VAL,
        .count = 1,
        .controls = &ctrl,
    };
    if (xioctl(fd, request, &ctrls) == -1)
    {
        return TRUE;
    }

    *value = ctrl.value;
    return FALSE;
}

static inline const struct media_v2_pad *
get_device_pad(const struct device_data *dev, uint32_t id)
{
    int i;
    for (i = 0; i < dev->num_pads; i++)
    {
        if (dev->pads[i].id == id)
        {
            return &dev->pads[i];
        }
    }
    return NULL;
}

/* TODO - get media device pad and video device pad and set up link */
static gboolean
setup_link(const struct device_data *dev, uint32_t source_pad_id,
           uint32_t sink_pad_id, gboolean enabled)
{
    const struct media_v2_pad *source_pad = get_device_pad(dev, source_pad_id);
    g_return_val_if_fail(source_pad, FALSE);

    const struct media_v2_pad *sink_pad = get_device_pad(dev, sink_pad_id);
    g_return_val_if_fail(sink_pad, FALSE);

    struct media_link_desc link = {};
    link.flags = enabled ? MEDIA_LNK_FL_ENABLED : 0;
    link.source.entity = source_pad->entity_id;
    link.source.index = 0;
    link.sink.entity = sink_pad->entity_id;
    link.sink.index = 0;
    if (xioctl(dev->fd, MEDIA_IOC_SETUP_LINK, &link) == -1)
    {
        g_printerr("MEDIA_IOC_SETUP_LINK: %d %s", errno, strerror(errno));
        return FALSE;
    }

    return TRUE;
}

static int alloc_device(struct device_data *dev)
{
    int rc = -1;
    g_print("topology x1\n");
    rc = xioctl(dev->fd, MEDIA_IOC_G_TOPOLOGY, &dev->topology);
    if (rc < 0)
    {
        g_printerr("topology x1 error %d %d %s\n", rc,
                   errno, strerror(errno));
        goto out;
    }
    dev->entities =
        calloc(dev->topology.num_entities,
               sizeof(struct media_v2_entity));
    dev->num_entities = dev->topology.num_entities;
    dev->interfaces =
        calloc(dev->topology.num_interfaces,
               sizeof(struct media_v2_interface));
    dev->num_interfaces = dev->topology.num_interfaces;
    dev->pads =
        calloc(dev->topology.num_pads, sizeof(struct media_v2_pad));
    dev->num_pads = dev->topology.num_pads;
    dev->links =
        calloc(dev->topology.num_links, sizeof(struct media_v2_link));
    dev->num_links = dev->topology.num_links;
    dev->topology.ptr_entities = (uint64_t)dev->entities;
    dev->topology.ptr_interfaces = (uint64_t)dev->interfaces;
    dev->topology.ptr_pads = (uint64_t)dev->pads;
    dev->topology.ptr_links = (uint64_t)dev->links;
    g_print("topology x2\n");
    rc = xioctl(dev->fd, MEDIA_IOC_G_TOPOLOGY, &dev->topology);
    if (rc < 0)
        goto out;

    g_print("device info\n");
    rc = xioctl(dev->fd, MEDIA_IOC_DEVICE_INFO, &dev->info);
out:
    return rc;
}

static void subdev_set_mode(int subdev_fd, int width,
		int height,
		uint32_t bus_code,
		struct v4l2_fract frame_interval)
{
  struct v4l2_subdev_frame_interval interval = {};
  struct v4l2_subdev_format fmt = {};
  gboolean frame_is_set;
  interval.pad = 0;
  interval.interval = frame_interval;
  if (xioctl(subdev_fd,
			  VIDIOC_SUBDEV_S_FRAME_INTERVAL,
			  &interval) == -1)
	  g_error("VIDIOC_SUBDEV_S_FRAME_INTERVAL %d %s\n",
			  errno, strerror(errno));
  frame_is_set =
	  interval.interval.numerator ==
	  frame_interval.numerator &&
	  interval.interval.denominator ==
	  frame_interval.denominator;

  fmt.pad = 0;
  fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
  fmt.format.width = width;
  fmt.format.height = height;
  fmt.format.code =
	  bus_code;
  fmt.format.field = V4L2_FIELD_ANY;
  if (xioctl(subdev_fd, VIDIOC_SUBDEV_S_FMT, &fmt) == -1)
	  g_error ("VIDIOC_SUBDEV_S_FMT: %d %s\n", errno, strerror(errno));
  if (!frame_is_set) {
  if (xioctl(subdev_fd,
			  VIDIOC_SUBDEV_S_FRAME_INTERVAL,
			  &interval) == -1)
	  g_error("VIDIOC_SUBDEV_S_FRAME_INTERVAL %d %s\n",
			  errno, strerror(errno));

  }
}
int
get_camera_config_no(const char *conf_name)
{
	int i;
	for (i = 0;
		i < sizeof(configurations)/sizeof(configurations[0]); i++) {
		struct camera_config *d = &configurations[i];
		if (!strncmp(d->conf_name, conf_name, sizeof(d->conf_name)))
			return i;
	}
	return -1;
}
static struct camera_config *
get_camera_config(const char *conf_name)
{
	int conf = get_camera_config_no(conf_name);
	if (conf < 0)
		return NULL;
	else
		return &configurations[conf];
}
static struct camera_data *
get_camera_data(const char *conf_name)
{
	int conf = get_camera_config_no(conf_name);
	if (conf < 0)
		return NULL;
	else {
		cam_store[conf].conf = &configurations[conf];
		return &cam_store[conf];
	}
}
void gst_pinesrcbin_configure_device(GstPineSrcBin * self, const char *conf_name)
{
  struct camera_config *conf;
  struct camera_data *cam_data;
  self->media_list = NULL;
  struct dirent *dir;
  conf = get_camera_config(conf_name);
  if (!conf)
	  g_error("invalid camera configuration name %s\n", conf_name);
  cam_data = get_camera_data(conf_name);
  g_assert(cam_data);
  GST_DEBUG_OBJECT (self, "Scanning for media devices");
  DIR *d = opendir ("/dev");
  while ((dir = readdir (d)) != NULL) {
    if (strncmp (dir->d_name, "media", 5) == 0) {
      char path[261];
      int fd;
      snprintf (path, 261, "/dev/%s", dir->d_name);
      fd = open (path, O_RDWR);
      GST_DEBUG_OBJECT (self, "Opened device:  %s: fd=%d\n",  path, fd);
      if (fd >= 0) {
        int rc = -1;
        struct device_data *dev = g_new0 (struct device_data, 1);
        if (!dev)
          goto out;
        dev->fd = fd;
        strncpy (dev->path, path, sizeof (dev->path));
        rc = alloc_device(dev);
        if (rc < 0)
          goto out;
        self->media_list = g_list_append(self->media_list, dev);
        GST_DEBUG_OBJECT (self, "added device %s %s\n", dev->path, dev->info.driver);
      out:
        if (rc < 0 && dev)
          g_free (dev);
      }
    }
  }
  /* TODO: configure file */
  self->media_if = gst_pinesrcbin_find_media_device (self, "sun6i-csi");
  if (!self->media_if)
    g_error ("can't find device sun6i-csi\n");
  /* sink entity */
  self->v4l2entity =
      gst_pinesrcbin_find_media_entity (self->media_if, "sun6i-csi");
  if (!self->v4l2entity)
    g_error ("can't find entity sun6i-csi\n");
  GST_DEBUG_OBJECT (self, "sink entity: %d %s\n", self->v4l2entity->id, self->v4l2entity->name);
  /* This is sink pad (for host controller) */
  self->v4l2pad =
      gst_pinesrcbin_get_pad_from_entity (self->media_if, self->v4l2entity->id);
  GST_DEBUG_OBJECT (self, "sink pad: %d\n", self->v4l2pad->id);
  if (!self->v4l2pad)
    g_error ("can't find interface pad sun6i-csi\n");
  self->v4l2interface =
      gst_pinesrcbin_get_entity_interface (self->media_if,
      self->v4l2entity->id);
  if (!self->v4l2interface)
    g_printerr ("can't find interface sun6i-csi\n");
  GST_DEBUG_OBJECT (self, "sink interface: %d\n", self->v4l2interface->id);
  if (!get_device_path (self->v4l2interface->devnode,
          self->devnode_path, sizeof (self->devnode_path)))
    g_error ("Failed to get devnode path\n");
  GST_DEBUG_OBJECT (self, "sink interface devnode path: %s\n", self->devnode_path);
  g_print ("devnode: %s\n", self->devnode_path);
  /* /dev/video device fd */
  self->fd = open (self->devnode_path, O_RDWR);
  if (self->fd < 0)
    g_error ("failed to open %s\n", self->devnode_path);
  /* TODO: source pad? sink pad? Need to disable active link*/
//  setup_link(self->media_if, ,self->v4l2interface->pad_id, false);
  disable_all_links(self->media_if);
  cam_data->entity = gst_pinesrcbin_find_media_entity(self->media_if, conf->camera_name);
  GST_DEBUG_OBJECT (self, "camera: %s: source interface entity %d\n", conf->camera_name, cam_data->entity->id);
  
  cam_data->interface =
      gst_pinesrcbin_get_entity_interface (self->media_if,
      cam_data->entity->id);
  GST_DEBUG_OBJECT (self, "camera: %s: source interface %d\n", conf->camera_name, cam_data->interface->id);
  if (!get_device_path (cam_data->interface->devnode,
          cam_data->devnode_path, sizeof (cam_data->devnode_path)))
    g_error ("Failed to get subdevice devnode path for %s\n", conf->camera_name);
  GST_DEBUG_OBJECT (self, "source subdevice %s\n", cam_data->devnode_path);
  cam_data->subdev_fd = open (cam_data->devnode_path, O_RDWR);
  if (cam_data->subdev_fd < 0)
	  g_error ("Failed to open subdevice: %s\n", cam_data->devnode_path);
  
  self->autofocus = FALSE;
  if (query_autofocus (cam_data->subdev_fd)) {
    uint32_t enable = 1U;
    g_print ("autofocus possible\n");
    self->autofocus = TRUE;
    camera_control (cam_data->subdev_fd, V4L2_CID_FOCUS_AUTO, VIDIOC_S_EXT_CTRLS, &enable);
  }
  setup_gain (cam_data->subdev_fd, &cam_data->gain_ctrl, &cam_data->gain_max);
  /* now configuring camera modes */
  GST_DEBUG_OBJECT (self, "configuring camera modes\n");
  subdev_set_mode(cam_data->subdev_fd, conf->width,
		  conf->height, conf->bus_code_video,
		  conf->frame_interval);
}