#include "ex_tp2825.h"
#include "camera/sdrv-cam-os-def.h"
//#define SENSOR_OV2K10  1
#define  SENSOR_OVX3D  1
#define  tp2825_DEVICE_ID  0x44   //High 2825 7bit Address
static uint32_t s_tp2825_width = 1920;
static uint32_t s_tp2825_height = 1080;
#if SENSOR_OV2K10
static uint32_t s_tp2825_fps = 30;
#else
static uint32_t s_tp2825_fps = 29;
#endif


enum tp2825_status {
    SERSOR_IDLE,
    SERSOR_STREAM_ON,
};

struct tp2825_dev {
    struct vdev_device vdev;
    void *i2c_handle;
    uint8_t deser_addr;
    uint8_t serder_addr;
    uint32_t gpio_pwr; // power supply pin
    uint32_t gpio_rst; // reset pin
    osMutexId_t lock;
    uint32_t status;
};

static int tp2825_write_reg(struct tp2825_dev *sensor, uint8_t reg, uint8_t val)
{
    uint8_t buf[2];
    int ret = 0;

	//sensor_info("%s: addr=0x%x, reg=0x%x, val=0x%x\n", __func__,sensor->deser_addr, reg, val);
    buf[0] = reg;
    buf[1] = val;
    ret = i2c_write(sensor->i2c_handle, sensor->deser_addr, buf, 2);
    if (ret < 0) {
        sensor_err("%s: error: reg=0x%x, val=%x\n", __func__, reg, val);
        return ret;
    }
    return ret;
}

static int tp2825_read_reg(struct tp2825_dev *sensor, uint8_t reg, uint8_t *val)
{
    uint8_t buf[1];
    int ret = 0;

    memset(buf, 0, sizeof(buf));
    ret = i2c_write_read(sensor->i2c_handle, sensor->deser_addr, &reg, 1, buf, 1);
    if (ret < 0) {
        sensor_err("%s: error: read reg=0x%02x\n", __func__, reg);
        return ret;
    }

    *val = buf[0];
    return 0;
}

__UNUSED static int tp2825_check_chip_id(struct tp2825_dev  *sensor)
{
    int ret = 0;
    uint8_t val[2];
    uint16_t chip_id = 0;

    ret = tp2825_read_reg(sensor, 0xfe, &val[0]);
    ret |= tp2825_read_reg(sensor, 0xff, &val[1]);
    chip_id = val[1]<<8 | val[0];
    if(chip_id != 0x2850) {
      ret = -1;
    }
    sensor_info("%s 0x%x 0x%x 0x%x\n", __func__, val[0],val[1], chip_id);
    return ret;
}

static int tp2825_init_setup(struct tp2825_dev  *sensor)
{

    sensor_info("%s \r\n",__func__ );
#if SENSOR_OV2K10
	tp2825_write_reg(sensor,0x0,0x11);
	tp2825_write_reg(sensor,0x1,0x7E);
	tp2825_write_reg(sensor,0x2,0x40);
	tp2825_write_reg(sensor,0x3,0x03);
	tp2825_write_reg(sensor,0x4,0x00);
	tp2825_write_reg(sensor,0x5,0x00);
	tp2825_write_reg(sensor,0x6,0x32);
	tp2825_write_reg(sensor,0x7,0xC0);
	tp2825_write_reg(sensor,0x8,0x00);
	tp2825_write_reg(sensor,0x9,0x24);
	tp2825_write_reg(sensor,0xA,0x48);
	tp2825_write_reg(sensor,0xB,0xC0);
	tp2825_write_reg(sensor,0xC,0x03);
	tp2825_write_reg(sensor,0xD,0x50);
	tp2825_write_reg(sensor,0xE,0x00);
	tp2825_write_reg(sensor,0xF,0x0);
	tp2825_write_reg(sensor,0x10,0x00);
	tp2825_write_reg(sensor,0x11,0x40);
	tp2825_write_reg(sensor,0x12,0x40);
	tp2825_write_reg(sensor,0x13,0x00);
	tp2825_write_reg(sensor,0x14,0x00);
	tp2825_write_reg(sensor,0x15,0x03);
	tp2825_write_reg(sensor,0x16,0xD2);
	tp2825_write_reg(sensor,0x17,0x80);
	tp2825_write_reg(sensor,0x18,0x29);
	tp2825_write_reg(sensor,0x19,0x38);
	tp2825_write_reg(sensor,0x1A,0x47);
	tp2825_write_reg(sensor,0x1B,0x01);
	tp2825_write_reg(sensor,0x1C,0x08);
	tp2825_write_reg(sensor,0x1D,0x98);
	tp2825_write_reg(sensor,0x1E,0x80);
	tp2825_write_reg(sensor,0x1F,0x80);
	tp2825_write_reg(sensor,0x20,0x30);
	tp2825_write_reg(sensor,0x21,0x84);
	tp2825_write_reg(sensor,0x22,0x36);
	tp2825_write_reg(sensor,0x23,0x3C);
	tp2825_write_reg(sensor,0x24,0x04);
	tp2825_write_reg(sensor,0x25,0xFF);
	tp2825_write_reg(sensor,0x26,0x05);
	tp2825_write_reg(sensor,0x27,0x2D);
	tp2825_write_reg(sensor,0x28,0x00);
	tp2825_write_reg(sensor,0x29,0x48);
	tp2825_write_reg(sensor,0x2A,0x30);
	tp2825_write_reg(sensor,0x2B,0x60);
	tp2825_write_reg(sensor,0x2C,0x0A);
	tp2825_write_reg(sensor,0x2D,0x30);
	tp2825_write_reg(sensor,0x2E,0x70);
	tp2825_write_reg(sensor,0x2F,0x00);
	tp2825_write_reg(sensor,0x30,0x48);
	tp2825_write_reg(sensor,0x31,0xBB);
	tp2825_write_reg(sensor,0x32,0x2E);
	tp2825_write_reg(sensor,0x33,0x90);
	tp2825_write_reg(sensor,0x34,0x00);
	tp2825_write_reg(sensor,0x35,0x05);
	tp2825_write_reg(sensor,0x36,0xDC);
	tp2825_write_reg(sensor,0x37,0x00);
	tp2825_write_reg(sensor,0x38,0x40);
	tp2825_write_reg(sensor,0x39,0x1C);
	tp2825_write_reg(sensor,0x3A,0x32);
	tp2825_write_reg(sensor,0x3B,0x26);
	tp2825_write_reg(sensor,0x3C,0x00);
	tp2825_write_reg(sensor,0x3D,0x60);
	tp2825_write_reg(sensor,0x3E,0x00);
	tp2825_write_reg(sensor,0x3F,0x00);
	tp2825_write_reg(sensor,0x40,0x00);
	tp2825_write_reg(sensor,0x41,0x00);
	tp2825_write_reg(sensor,0x42,0x00);
	tp2825_write_reg(sensor,0x43,0x00);
	tp2825_write_reg(sensor,0x44,0x00);
	tp2825_write_reg(sensor,0x45,0x00);
	tp2825_write_reg(sensor,0x46,0x00);
	tp2825_write_reg(sensor,0x47,0x00);
	tp2825_write_reg(sensor,0x48,0x00);
	tp2825_write_reg(sensor,0x49,0x00);
	tp2825_write_reg(sensor,0x4A,0x00);
	tp2825_write_reg(sensor,0x4B,0x00);
	tp2825_write_reg(sensor,0x4C,0x43);
	tp2825_write_reg(sensor,0x4D,0x00);
	tp2825_write_reg(sensor,0x4E,0x0D);
	tp2825_write_reg(sensor,0x4F,0x00);

	tp2825_write_reg(sensor,0xF0,0x00);
	tp2825_write_reg(sensor,0xF1,0x00);
	tp2825_write_reg(sensor,0xF2,0x00);
	tp2825_write_reg(sensor,0xF3,0x00);
	tp2825_write_reg(sensor,0xF4,0x20);
	tp2825_write_reg(sensor,0xF5,0x10);
	tp2825_write_reg(sensor,0xF6,0x00);
	tp2825_write_reg(sensor,0xF7,0x44);
	tp2825_write_reg(sensor,0xF8,0x00);
	tp2825_write_reg(sensor,0xF9,0x00);
	tp2825_write_reg(sensor,0xFA,0x0b);
	tp2825_write_reg(sensor,0xFB,0x00);
	tp2825_write_reg(sensor,0xFC,0x00);
	tp2825_write_reg(sensor,0xFD,0x80);
	tp2825_write_reg(sensor,0xFE,0x28);
	tp2825_write_reg(sensor,0xF0,0x00);
	tp2825_write_reg(sensor,0xF1,0x00);
	tp2825_write_reg(sensor,0xF2,0x00);
	tp2825_write_reg(sensor,0xF3,0x00);
	tp2825_write_reg(sensor,0xF4,0x20);
	tp2825_write_reg(sensor,0xF5,0x10);
	tp2825_write_reg(sensor,0xF6,0x00);
	tp2825_write_reg(sensor,0xF7,0x44);
	tp2825_write_reg(sensor,0xF8,0x00);
	tp2825_write_reg(sensor,0xF9,0x00);
	tp2825_write_reg(sensor,0xFA,0x0b);
	tp2825_write_reg(sensor,0xFB,0x00);
	tp2825_write_reg(sensor,0xFC,0x00);
	tp2825_write_reg(sensor,0xFD,0x80);
	tp2825_write_reg(sensor,0xFE,0x28);
	tp2825_write_reg(sensor,0x40,0x08); //mpage select
	tp2825_write_reg(sensor,0x13,0x04);
	tp2825_write_reg(sensor,0x40,0x00); //back to decoder page
#else
	//config 0x3D 1920X1080 29 FPS  settings
	tp2825_write_reg(sensor,0x00,0x11);
	tp2825_write_reg(sensor,0x02,0x40);
	tp2825_write_reg(sensor,0x03,0x03);
	tp2825_write_reg(sensor,0x05,0x00);
	tp2825_write_reg(sensor,0x06,0x32);
	tp2825_write_reg(sensor,0x07,0xC0);
	tp2825_write_reg(sensor,0x08,0x00);
	tp2825_write_reg(sensor,0x09,0x24);
	tp2825_write_reg(sensor,0x0A,0x48);
	tp2825_write_reg(sensor,0x0B,0xC0);
	tp2825_write_reg(sensor,0x0C,0x03);
	tp2825_write_reg(sensor,0x0D,0x50);
	tp2825_write_reg(sensor,0x0E,0x00);
	tp2825_write_reg(sensor,0x0F,0x00);
	tp2825_write_reg(sensor,0x10,0x00);
	tp2825_write_reg(sensor,0x11,0x40);
	tp2825_write_reg(sensor,0x12,0x40);
	tp2825_write_reg(sensor,0x13,0x00);
	tp2825_write_reg(sensor,0x14,0x00);
	tp2825_write_reg(sensor,0x15,0x13);
	tp2825_write_reg(sensor,0x16,0xE8);
	tp2825_write_reg(sensor,0x17,0x80);
	tp2825_write_reg(sensor,0x18,0x54);
	tp2825_write_reg(sensor,0x19,0x38);
	tp2825_write_reg(sensor,0x1A,0x47);
	tp2825_write_reg(sensor,0x1B,0x01);
	tp2825_write_reg(sensor,0x1C,0x09);
	tp2825_write_reg(sensor,0x1D,0xab);
	tp2825_write_reg(sensor,0x1E,0x80);
	tp2825_write_reg(sensor,0x1F,0x80);
	tp2825_write_reg(sensor,0x20,0x30);
	tp2825_write_reg(sensor,0x21,0x84);
	tp2825_write_reg(sensor,0x22,0x36);
	tp2825_write_reg(sensor,0x23,0x3C);
	tp2825_write_reg(sensor,0x24,0x04);
	tp2825_write_reg(sensor,0x25,0xFF);
	tp2825_write_reg(sensor,0x26,0x05);
	tp2825_write_reg(sensor,0x27,0x2D);
	tp2825_write_reg(sensor,0x28,0x00);
	tp2825_write_reg(sensor,0x29,0x48);
	tp2825_write_reg(sensor,0x2A,0x30);
	tp2825_write_reg(sensor,0x2B,0x60);
	tp2825_write_reg(sensor,0x2C,0x0A);
	tp2825_write_reg(sensor,0x2D,0x30);
	tp2825_write_reg(sensor,0x2E,0x70);
	tp2825_write_reg(sensor,0x2F,0x00);
	tp2825_write_reg(sensor,0x30,0x48);
	tp2825_write_reg(sensor,0x31,0xBB);
	tp2825_write_reg(sensor,0x32,0x2E);
	tp2825_write_reg(sensor,0x33,0x90);
	tp2825_write_reg(sensor,0x34,0x00);
	tp2825_write_reg(sensor,0x35,0x14);
	tp2825_write_reg(sensor,0x36,0xB5);
	tp2825_write_reg(sensor,0x37,0x00);
	tp2825_write_reg(sensor,0x38,0x40);
	tp2825_write_reg(sensor,0x39,0x1C);
	tp2825_write_reg(sensor,0x3A,0x32);
	tp2825_write_reg(sensor,0x3B,0x26);
	tp2825_write_reg(sensor,0x3C,0x00);
	tp2825_write_reg(sensor,0x3D,0x60);
	tp2825_write_reg(sensor,0x3E,0x00);
	tp2825_write_reg(sensor,0x3F,0x00);
	tp2825_write_reg(sensor,0x40,0x00);
	tp2825_write_reg(sensor,0x41,0x00);
	tp2825_write_reg(sensor,0x42,0x00);
	tp2825_write_reg(sensor,0x43,0x00);
	tp2825_write_reg(sensor,0x44,0x00);
	tp2825_write_reg(sensor,0x45,0x00);
	tp2825_write_reg(sensor,0x46,0x00);
	tp2825_write_reg(sensor,0x47,0x00);
	tp2825_write_reg(sensor,0x48,0x00);
	tp2825_write_reg(sensor,0x49,0x00);
	tp2825_write_reg(sensor,0x4A,0x00);
	tp2825_write_reg(sensor,0x4B,0x00);
	tp2825_write_reg(sensor,0x4C,0x43);
	tp2825_write_reg(sensor,0x4D,0x00);
	tp2825_write_reg(sensor,0x4E,0x0D);
	tp2825_write_reg(sensor,0x4F,0x00);
	tp2825_write_reg(sensor,0xF0,0x00);
	tp2825_write_reg(sensor,0xF1,0x00);
	tp2825_write_reg(sensor,0xF2,0x00);
	tp2825_write_reg(sensor,0xF3,0x00);
	tp2825_write_reg(sensor,0xF4,0x20);
	tp2825_write_reg(sensor,0xF5,0x10);
	tp2825_write_reg(sensor,0xF6,0x00);
	tp2825_write_reg(sensor,0xF7,0x44);
	tp2825_write_reg(sensor,0xF8,0x00);
	tp2825_write_reg(sensor,0xF9,0x00);
	tp2825_write_reg(sensor,0xFA,0x0b);
	tp2825_write_reg(sensor,0xFB,0x00);
	tp2825_write_reg(sensor,0xFC,0x00);
	tp2825_write_reg(sensor,0xFD,0x80);
	tp2825_write_reg(sensor,0xFE,0x28);
	tp2825_write_reg(sensor,0x40,0x08); // mpage select
	tp2825_write_reg(sensor,0x13,0x04);
	tp2825_write_reg(sensor,0x40,0x00);//back to decoder page
#endif
	return 0;
}

static int tp2825_s_power(struct vdev_device *vdev, int enable)
{
    struct tp2825_dev *sensor;
    uint32_t pin;
    if (!vdev) {
        return -1;
    }

    sensor = (struct tp2825_dev *)vdev->priv_data;
    pin = sensor->gpio_rst;
    sensor_info("%s: pin %d\r\n", __func__, pin);

    return 0;
}
static int tp2825_s_rst(struct vdev_device *vdev, int enable)
{

    struct tp2825_dev *sensor;
    uint32_t pin;
    if (!vdev) {
        return -1;
    }
    sensor = (struct tp2825_dev *)vdev->priv_data;
    pin = sensor->gpio_rst;
    sensor_info("%s: pin %d,  enable %d\r\n", __func__, pin, enable);
    gpio_set(NULL, pin, enable ? true : false);

    return 0;
}



static int tp2825_read_regs(struct tp2825_dev *sensor)
{
    uint8_t val = 0;
     uint8_t reg;
    //usleep_range(10000, 11000);

    tp2825_write_reg(sensor, 0x40, 0x00);
    for(reg =0 ; reg<0xff;  reg++)
    {
           tp2825_read_reg(sensor, reg,&val);
           sensor_info("reg:0x%x val 0x%x\n\r", reg,val);
   }
   sensor_info("tp2825 read switch page\n\r");
    tp2825_write_reg(sensor, 0x40, 0x08);
    for(int reg =0 ;reg<0xff;reg++)
    {
           tp2825_read_reg(sensor, reg,&val);
           sensor_info("reg:0x%x val 0x%x\n\r",reg,val);
   }

	return 0;
}


static int tp2825_enum_mbus_fmt(struct vdev_device *vdev,
                               struct vdev_mbus_framefmt *fme)
{
    struct tp2825_dev *sensor;

    if ((vdev == NULL) || (fme == NULL)) {
        return -1;
    }
	sensor_err("%s: \r\n", __func__);
    sensor = (struct tp2825_dev *)vdev->priv_data;

    if (fme->index > 0) {
        fme->index = 0;
        return -1;
    }

    fme->code = sensor->vdev.fmt.code;
    fme->field = sensor->vdev.fmt.field;
    fme->colorspace = sensor->vdev.fmt.colorspace;
    return 0;
}

static int tp2825_enum_frame_size(struct vdev_device *vdev,
                                 struct vdev_frame_size_enum *fse)
{
    struct tp2825_dev *sensor;

	sensor_err("%s: \r\n", __func__);
    if (!vdev || !fse) {
        return -1;
    }

    sensor = (struct tp2825_dev *)vdev->priv_data;

    if (fse->index > 0) {
        fse->index = 0;
        return -1;
    }

    fse->min_width = sensor->vdev.fmt.width;
    fse->max_width = sensor->vdev.fmt.width;
    fse->min_height = sensor->vdev.fmt.height;
    fse->max_height = sensor->vdev.fmt.height;

    return 0;
}


static int tp2825_enum_frame_interval(struct vdev_device *vdev,
                                     struct vdev_frame_interval_enum *fie)
{

	sensor_err("%s: \r\n", __func__);
    return 0;
}

static int tp2825_get_frame_interval(struct vdev_device *vdev,
                                    struct vdev_fract *fi)
{
    if (!vdev || !fi) {
        return -1;
    }

	sensor_err("%s: \r\n", __func__);
    *fi = vdev->frame_interval;
    return 0;
}

static int tp2825_set_frame_interval(struct vdev_device *vdev,
                                    struct vdev_fract fi)
{

	sensor_err("%s: \r\n", __func__);
	return 0;
}

static int tp2825_get_fmt(struct vdev_device *vdev,
                         struct vdev_mbus_framefmt *fmt)
{
    if (!vdev) {
        return -1;
    }

    sensor_err("%s: \r\n", __func__);
    *fmt = vdev->fmt;
    return 0;
}

static int tp2825_set_fmt(struct vdev_device *vdev,
                         struct vdev_mbus_framefmt fmt)
{

	sensor_err("%s: \r\n", __func__);
	return 0;
}

static int tp2825_s_stream(struct vdev_device *vdev, int enable)
{
    int ret = 0;
    struct tp2825_dev *sensor;

    if (!vdev) {
        return -1;
    }

    sensor_err("%s: \r\n", __func__);
    sensor = (struct tp2825_dev *)vdev->priv_data;
    osMutexAcquire(sensor->lock, 0);

    if (enable == 1) {
        if (sensor->status == SERSOR_STREAM_ON) {
            sensor_info("%s: already stream on\r\n", __func__);
            goto exit;
        }

        ret = tp2825_init_setup(sensor);
        if (ret < 0)
            goto exit;

        mdelay(800); //delay 700 ms
        tp2825_read_regs(sensor);

        sensor->status = SERSOR_STREAM_ON;

    } else {
        sensor->status = SERSOR_IDLE;
    }

exit:
    osMutexRelease(sensor->lock);
    return ret;
}

static int tp2825_deinit(struct vdev_device *vdev)
{
    struct tp2825_dev *sensor;

    if (!vdev) {
        return -1;
    }

    sensor = (struct tp2825_dev *)vdev->priv_data;
    osMutexDelete(sensor->lock);
    free(sensor);

    return 0;
}

static const struct vdev_dev_ops tp2825_vdev_ops = {
    .s_power = tp2825_s_power,
    .enum_mbus_fmt = tp2825_enum_mbus_fmt,
    .enum_frame_size = tp2825_enum_frame_size,
    .enum_frame_interval = tp2825_enum_frame_interval,
    .g_frame_interval = tp2825_get_frame_interval,
    .s_frame_interval = tp2825_set_frame_interval,
    .get_fmt = tp2825_get_fmt,
    .set_fmt = tp2825_set_fmt,
    .s_stream = tp2825_s_stream,
    .close = tp2825_deinit,
};

struct tp2825_dev *g_tp2825 = NULL;

struct vdev_device *tp2825_init(void *init_data)
{

    struct tp2825_dev *sensor;
    struct camera_res_cfg *cfg = (struct camera_res_cfg *)init_data;

    sensor = pvPortMalloc(sizeof(struct tp2825_dev));
    if (!sensor || !cfg)
        return NULL;

    sensor_err("%s: \r\n", __func__);
    memset(sensor, 0, sizeof(struct tp2825_dev));
    sensor->i2c_handle = cfg->i2c_handle;
    sensor->deser_addr = tp2825_DEVICE_ID;
    sensor->gpio_rst =  cfg->gpio[0]; //rst pin
    sensor->gpio_pwr = cfg->gpio[1]; //pwr pin

    sensor->vdev.priv_data = (void *)sensor;
    sensor->lock = osMutexNew(NULL);


    tp2825_s_rst(&sensor->vdev, 1);
    usleep_range(1000, 1100);

    tp2825_s_rst(&sensor->vdev,  0);
    usleep_range(1000, 1000);
    tp2825_s_rst(&sensor->vdev, 1);

    //ret = tp2825_check_chip_id(sensor);

    tp2825_s_stream(&sensor->vdev, 0);

    sensor->vdev.ep.bus_type = VDEV_MBUS_BT1120SDR;
    sensor->vdev.frame_interval.numerator = 1;
    sensor->vdev.frame_interval.denominator = s_tp2825_fps;
    sensor->vdev.fmt.width = s_tp2825_width;
    sensor->vdev.fmt.height = s_tp2825_height;
    sensor->vdev.fmt.code = VDEV_MBUS_FMT_UYVY16; //VDEV_MBUS_FMT_UYVY16   VDEV_MBUS_FMT_UYVY16
    sensor->vdev.fmt.field = VDEV_FIELD_NONE;
    sensor->vdev.fmt.colorspace = VDEV_COLORSPACE_SRGB;
    sensor->vdev.ops = tp2825_vdev_ops;

    g_tp2825 = sensor;
    return &sensor->vdev;
}