using System.Net;
using DotNext;

namespace Peripherals.CameraClient;

static class CameraAddr
{
    public const UInt32 BASE = 0x7000_0000;

    public const UInt32 STORE_ADDR = BASE + 0x12;
    public const UInt32 STORE_NUM = BASE + 0x13;
    public const UInt32 CAPTURE_ON = BASE + 0x14;
}

class Camera
{
    private static readonly NLog.Logger logger = NLog.LogManager.GetCurrentClassLogger();

    readonly int timeout = 2000;

    readonly int port;
    readonly string address;
    private IPEndPoint ep;


    const uint CAM_I2C_ADDR = 0x3C;
    const Peripherals.I2cClient.I2cProtocol CAM_PROTO = Peripherals.I2cClient.I2cProtocol.SCCB;

    const UInt16 ISP_INPUT_SIZE_X = 2624;
    const UInt16 ISP_INPUT_SIZE_Y = 1952;
    const UInt16 X_OFFSET = 16;
    const UInt16 Y_OFFSET = 4;
    const UInt16 X_OUTPUT_SIZE = 640;
    const UInt16 Y_OUTPUT_SIZE = 480;
    const UInt16 X_TOTAL_OUTPUT_SIZE = 2844;
    const UInt16 Y_TOTAL_OUTPUT_SIZE = 1968;
    const UInt16 PISYCAL_PIXEL_START_X = 336;
    const UInt16 PISYCAL_PIXEL_START_Y = 434;
    const UInt16 PISYCAL_PIXEL_END_X = PISYCAL_PIXEL_START_X + ISP_INPUT_SIZE_X - 1;
    const UInt16 PISYCAL_PIXEL_END_Y = PISYCAL_PIXEL_START_Y + ISP_INPUT_SIZE_Y - 1;
    const byte PLL_MUX = 105;

    const UInt32 FrameAddr = 0x00;
    const UInt32 FrameLength = X_OUTPUT_SIZE * Y_OUTPUT_SIZE * 16 / 32;

    static byte[][] InitCmdData = new byte[][] {
        // Stop streaming
        new byte[] { 0x30, 0x08, 0x42 },
        new byte[] { 0x31, 0x03, 0x02 },
        new byte[] { 0x30, 0x17, 0xff },
        new byte[] { 0x30, 0x18, 0xff },
        new byte[] { 0x30, 0x37, 0x13 },
        new byte[] { 0x31, 0x08, 0x01 },
        new byte[] { 0x36, 0x30, 0x36 },
        new byte[] { 0x36, 0x31, 0x0e },
        new byte[] { 0x36, 0x32, 0xe2 },
        new byte[] { 0x36, 0x33, 0x12 },
        new byte[] { 0x36, 0x21, 0xe0 },
        new byte[] { 0x37, 0x04, 0xa0 },
        new byte[] { 0x37, 0x03, 0x5a },
        new byte[] { 0x37, 0x15, 0x78 },
        new byte[] { 0x37, 0x17, 0x01 },
        new byte[] { 0x37, 0x0b, 0x60 },
        new byte[] { 0x37, 0x05, 0x1a },
        new byte[] { 0x39, 0x05, 0x02 },
        new byte[] { 0x39, 0x06, 0x10 },
        new byte[] { 0x39, 0x01, 0x0a },
        new byte[] { 0x37, 0x31, 0x12 },
        new byte[] { 0x36, 0x00, 0x08 },
        new byte[] { 0x36, 0x01, 0x33 },
        new byte[] { 0x30, 0x2d, 0x60 },
        new byte[] { 0x36, 0x20, 0x52 },
        new byte[] { 0x37, 0x1b, 0x20 },
        new byte[] { 0x47, 0x1c, 0x50 },
        new byte[] { 0x3a, 0x13, 0x43 },
        new byte[] { 0x3a, 0x18, 0x00 },
        new byte[] { 0x3a, 0x19, 0xf8 },
        new byte[] { 0x36, 0x35, 0x13 },
        new byte[] { 0x36, 0x36, 0x03 },
        new byte[] { 0x36, 0x34, 0x40 },
        new byte[] { 0x36, 0x22, 0x01 },
        new byte[] { 0x3c, 0x01, 0x34 },
        new byte[] { 0x3c, 0x04, 0x28 },
        new byte[] { 0x3c, 0x05, 0x98 },
        new byte[] { 0x3c, 0x06, 0x00 },
        new byte[] { 0x3c, 0x07, 0x07 },
        new byte[] { 0x3c, 0x08, 0x00 },
        new byte[] { 0x3c, 0x09, 0x1c },
        new byte[] { 0x3c, 0x0a, 0x9c },
        new byte[] { 0x3c, 0x0b, 0x40 },
        // X_OFFSET/Y_OFFSET
        new byte[] { 0x38, 0x10, unchecked((byte)((X_OFFSET >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x11, unchecked((byte)(X_OFFSET & 0xFF)) },
        new byte[] { 0x38, 0x12, unchecked((byte)((Y_OFFSET >> 8) & 0xFF)) },
        new byte[] { 0x37, 0x08, 0x64 },
        new byte[] { 0x40, 0x01, 0x02 },
        new byte[] { 0x40, 0x05, 0x1a },
        new byte[] { 0x30, 0x00, 0x00 },
        new byte[] { 0x30, 0x04, 0xff },
        new byte[] { 0x43, 0x00, 0x00 }, // RAW: BGGR
        new byte[] { 0x50, 0x1f, 0x03 }, // ISP RAW DPC
        new byte[] { 0x44, 0x0e, 0x00 },
        new byte[] { 0x50, 0x00, 0x06 },
        new byte[] { 0x3a, 0x0f, 0x78 },
        new byte[] { 0x3a, 0x10, 0x68 },
        new byte[] { 0x3a, 0x1b, 0x78 },
        new byte[] { 0x3a, 0x1e, 0x68 },
        new byte[] { 0x3a, 0x11, 0xD0 },
        new byte[] { 0x3a, 0x1f, 0x40 },
        // LENC
        new byte[] { 0x58, 0x00, 0x23 }, new byte[] { 0x58, 0x01, 0x14 }, new byte[] { 0x58, 0x02, 0x0f },
        new byte[] { 0x58, 0x03, 0x0f }, new byte[] { 0x58, 0x04, 0x12 }, new byte[] { 0x58, 0x05, 0x26 },
        new byte[] { 0x58, 0x06, 0x0c }, new byte[] { 0x58, 0x07, 0x08 }, new byte[] { 0x58, 0x08, 0x05 },
        new byte[] { 0x58, 0x09, 0x05 }, new byte[] { 0x58, 0x0a, 0x08 }, new byte[] { 0x58, 0x0b, 0x0d },
        new byte[] { 0x58, 0x0c, 0x08 }, new byte[] { 0x58, 0x0d, 0x03 }, new byte[] { 0x58, 0x0e, 0x00 },
        new byte[] { 0x58, 0x0f, 0x00 }, new byte[] { 0x58, 0x10, 0x03 }, new byte[] { 0x58, 0x11, 0x09 },
        new byte[] { 0x58, 0x12, 0x07 }, new byte[] { 0x58, 0x13, 0x03 }, new byte[] { 0x58, 0x14, 0x00 },
        new byte[] { 0x58, 0x15, 0x01 }, new byte[] { 0x58, 0x16, 0x03 }, new byte[] { 0x58, 0x17, 0x08 },
        new byte[] { 0x58, 0x18, 0x0d }, new byte[] { 0x58, 0x19, 0x08 }, new byte[] { 0x58, 0x1a, 0x05 },
        new byte[] { 0x58, 0x1b, 0x06 }, new byte[] { 0x58, 0x1c, 0x08 }, new byte[] { 0x58, 0x1d, 0x0e },
        new byte[] { 0x58, 0x1e, 0x29 }, new byte[] { 0x58, 0x1f, 0x17 }, new byte[] { 0x58, 0x20, 0x11 },
        new byte[] { 0x58, 0x21, 0x11 }, new byte[] { 0x58, 0x22, 0x15 }, new byte[] { 0x58, 0x23, 0x28 },
        new byte[] { 0x58, 0x24, 0x46 }, new byte[] { 0x58, 0x25, 0x26 }, new byte[] { 0x58, 0x26, 0x08 },
        new byte[] { 0x58, 0x27, 0x26 }, new byte[] { 0x58, 0x28, 0x64 }, new byte[] { 0x58, 0x29, 0x26 },
        new byte[] { 0x58, 0x2a, 0x24 }, new byte[] { 0x58, 0x2b, 0x22 }, new byte[] { 0x58, 0x2c, 0x24 },
        new byte[] { 0x58, 0x2d, 0x24 }, new byte[] { 0x58, 0x2e, 0x06 }, new byte[] { 0x58, 0x2f, 0x22 },
        new byte[] { 0x58, 0x30, 0x40 }, new byte[] { 0x58, 0x31, 0x42 }, new byte[] { 0x58, 0x32, 0x24 },
        new byte[] { 0x58, 0x33, 0x26 }, new byte[] { 0x58, 0x34, 0x24 }, new byte[] { 0x58, 0x35, 0x22 },
        new byte[] { 0x58, 0x36, 0x22 }, new byte[] { 0x58, 0x37, 0x26 }, new byte[] { 0x58, 0x38, 0x44 },
        new byte[] { 0x58, 0x39, 0x24 }, new byte[] { 0x58, 0x3a, 0x26 }, new byte[] { 0x58, 0x3b, 0x28 },
        new byte[] { 0x58, 0x3c, 0x42 }, new byte[] { 0x58, 0x3d, 0xce },
        // AWB
        new byte[] { 0x51, 0x80, 0xff }, new byte[] { 0x51, 0x81, 0xf2 }, new byte[] { 0x51, 0x82, 0x00 },
        new byte[] { 0x51, 0x83, 0x14 }, new byte[] { 0x51, 0x84, 0x25 }, new byte[] { 0x51, 0x85, 0x24 },
        new byte[] { 0x51, 0x86, 0x09 }, new byte[] { 0x51, 0x87, 0x09 }, new byte[] { 0x51, 0x88, 0x09 },
        new byte[] { 0x51, 0x89, 0x75 }, new byte[] { 0x51, 0x8a, 0x54 }, new byte[] { 0x51, 0x8b, 0xe0 },
        new byte[] { 0x51, 0x8c, 0xb2 }, new byte[] { 0x51, 0x8d, 0x42 }, new byte[] { 0x51, 0x8e, 0x3d },
        new byte[] { 0x51, 0x8f, 0x56 }, new byte[] { 0x51, 0x90, 0x46 }, new byte[] { 0x51, 0x91, 0xf8 },
        new byte[] { 0x51, 0x92, 0x04 }, new byte[] { 0x51, 0x93, 0x70 }, new byte[] { 0x51, 0x94, 0xf0 },
        new byte[] { 0x51, 0x95, 0xf0 }, new byte[] { 0x51, 0x96, 0x03 }, new byte[] { 0x51, 0x97, 0x01 },
        new byte[] { 0x51, 0x98, 0x04 }, new byte[] { 0x51, 0x99, 0x12 }, new byte[] { 0x51, 0x9a, 0x04 },
        new byte[] { 0x51, 0x9b, 0x00 }, new byte[] { 0x51, 0x9c, 0x06 }, new byte[] { 0x51, 0x9d, 0x82 },
        new byte[] { 0x51, 0x9e, 0x38 },
        // Gamma
        new byte[] { 0x54, 0x80, 0x01 }, new byte[] { 0x54, 0x81, 0x08 }, new byte[] { 0x54, 0x82, 0x14 },
        new byte[] { 0x54, 0x83, 0x28 }, new byte[] { 0x54, 0x84, 0x51 }, new byte[] { 0x54, 0x85, 0x65 },
        new byte[] { 0x54, 0x86, 0x71 }, new byte[] { 0x54, 0x87, 0x7d }, new byte[] { 0x54, 0x88, 0x87 },
        new byte[] { 0x54, 0x89, 0x91 }, new byte[] { 0x54, 0x8a, 0x9a }, new byte[] { 0x54, 0x8b, 0xaa },
        new byte[] { 0x54, 0x8c, 0xb8 }, new byte[] { 0x54, 0x8d, 0xcd }, new byte[] { 0x54, 0x8e, 0xdd },
        new byte[] { 0x54, 0x8f, 0xea }, new byte[] { 0x54, 0x90, 0x1d },
        // CMX
        new byte[] { 0x53, 0x81, 0x1e }, new byte[] { 0x53, 0x82, 0x5b }, new byte[] { 0x53, 0x83, 0x08 },
        new byte[] { 0x53, 0x84, 0x0a }, new byte[] { 0x53, 0x85, 0x7e }, new byte[] { 0x53, 0x86, 0x88 },
        new byte[] { 0x53, 0x87, 0x7c }, new byte[] { 0x53, 0x88, 0x6c }, new byte[] { 0x53, 0x89, 0x10 },
        new byte[] { 0x53, 0x8a, 0x01 }, new byte[] { 0x53, 0x8b, 0x98 },
        // SDE
        new byte[] { 0x55, 0x80, 0x06 }, new byte[] { 0x55, 0x83, 0x40 }, new byte[] { 0x55, 0x84, 0x10 },
        new byte[] { 0x55, 0x89, 0x10 }, new byte[] { 0x55, 0x8a, 0x00 }, new byte[] { 0x55, 0x8b, 0xf8 },
        new byte[] { 0x50, 0x1d, 0x40 },
        // CIP
        new byte[] { 0x53, 0x00, 0x08 }, new byte[] { 0x53, 0x01, 0x30 }, new byte[] { 0x53, 0x02, 0x10 },
        new byte[] { 0x53, 0x03, 0x00 }, new byte[] { 0x53, 0x04, 0x08 }, new byte[] { 0x53, 0x05, 0x30 },
        new byte[] { 0x53, 0x06, 0x08 }, new byte[] { 0x53, 0x07, 0x16 }, new byte[] { 0x53, 0x09, 0x08 },
        new byte[] { 0x53, 0x0a, 0x30 }, new byte[] { 0x53, 0x0b, 0x04 }, new byte[] { 0x53, 0x0c, 0x06 },
        new byte[] { 0x50, 0x25, 0x00 },
        // 系统时钟分频
        new byte[] { 0x30, 0x35, 0x21 }, // 30fps
        new byte[] { 0x30, 0x36, PLL_MUX }, // PLL倍频
        new byte[] { 0x3c, 0x07, 0x08 },
        // 时序控制
        new byte[] { 0x38, 0x20, 0x41 }, // vflip
        new byte[] { 0x38, 0x21, 0x00 }, // mirror
        new byte[] { 0x38, 0x14, 0x11 }, // timing X inc
        new byte[] { 0x38, 0x15, 0x11 }, // timing Y inc
        // PISYCAL_PIXEL_START_X/Y
        new byte[] { 0x38, 0x00, unchecked((byte)((PISYCAL_PIXEL_START_X >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x01, unchecked((byte)(PISYCAL_PIXEL_START_X & 0xFF)) },
        new byte[] { 0x38, 0x02, unchecked((byte)((PISYCAL_PIXEL_START_Y >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x03, unchecked((byte)(PISYCAL_PIXEL_START_Y & 0xFF)) },
        // PISYCAL_PIXEL_END_X/Y
        new byte[] { 0x38, 0x04, unchecked((byte)((PISYCAL_PIXEL_END_X >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x05, unchecked((byte)(PISYCAL_PIXEL_END_X & 0xFF)) },
        new byte[] { 0x38, 0x06, unchecked((byte)((PISYCAL_PIXEL_END_Y >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x07, unchecked((byte)(PISYCAL_PIXEL_END_Y & 0xFF)) },
        // 输出像素个数
        new byte[] { 0x38, 0x08, unchecked((byte)((X_OUTPUT_SIZE >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x09, unchecked((byte)(X_OUTPUT_SIZE & 0xFF)) },
        new byte[] { 0x38, 0x0A, unchecked((byte)((Y_OUTPUT_SIZE >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x0B, unchecked((byte)(Y_OUTPUT_SIZE & 0xFF)) },
        // 总像素
        new byte[] { 0x38, 0x0C, unchecked((byte)((X_TOTAL_OUTPUT_SIZE >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x0D, unchecked((byte)(X_TOTAL_OUTPUT_SIZE & 0xFF)) },
        new byte[] { 0x38, 0x0E, unchecked((byte)((Y_TOTAL_OUTPUT_SIZE >> 8) & 0xFF)) },
        new byte[] { 0x38, 0x0F, unchecked((byte)(Y_TOTAL_OUTPUT_SIZE & 0xFF)) },
        new byte[] { 0x38, 0x13, unchecked((byte)(Y_OFFSET & 0xFF)) }, // Timing Voffset
        new byte[] { 0x36, 0x18, 0x00 },
        new byte[] { 0x36, 0x12, 0x29 },
        new byte[] { 0x37, 0x09, 0x52 },
        new byte[] { 0x37, 0x0c, 0x03 },
        new byte[] { 0x3a, 0x02, 0x17 },
        new byte[] { 0x3a, 0x03, 0x10 },
        new byte[] { 0x3a, 0x14, 0x17 },
        new byte[] { 0x3a, 0x15, 0x10 },
        new byte[] { 0x40, 0x04, 0x02 },
        new byte[] { 0x47, 0x13, 0x00 },
        new byte[] { 0x44, 0x07, 0x04 },
        new byte[] { 0x46, 0x0c, 0x20 },
        new byte[] { 0x48, 0x37, 0x22 },
        new byte[] { 0x38, 0x24, 0x02 },
        new byte[] { 0x50, 0x01, 0x80 }, // 关闭缩放
        new byte[] { 0x3b, 0x07, 0x0a },
        // 彩条测试使能
        new byte[] { 0x50, 0x3d, 0x00 },
        // 闪光灯禁用
        new byte[] { 0x30, 0x16, 0x00 },
        new byte[] { 0x30, 0x1c, 0x00 },
        new byte[] { 0x30, 0x19, 0x00 },
        new byte[] { 0x30, 0x31, 0x08 },
        new byte[] { 0x30, 0x2c, 0xC2 },
        new byte[] { 0x46, 0x00, 0xA0 },
        // for subsample=OFF
        new byte[] { 0x36, 0x18, 0x04 },
        new byte[] { 0x36, 0x12, 0x2b },
        new byte[] { 0x37, 0x09, 0x12 },
        new byte[] { 0x37, 0x0c, 0x00 },
        // Start streaming
        new byte[] { 0x30, 0x08, 0x02 }
    };


    /// <summary>
    /// 初始化摄像头客户端
    /// </summary>
    /// <param name="address">摄像头设备IP地址</param>
    /// <param name="port">摄像头设备端口</param>
    /// <param name="timeout">超时时间（毫秒）</param>
    public Camera(string address, int port, int timeout = 2000)
    {
        if (timeout < 0)
            throw new ArgumentException("Timeout couldn't be negative", nameof(timeout));
        this.address = address;
        this.port = port;
        this.ep = new IPEndPoint(IPAddress.Parse(address), port);
        this.timeout = timeout;
    }

    public async ValueTask<Result<bool>> Init()
    {
        {
            var ret = await UDPClientPool.WriteAddr(this.ep, 2, CameraAddr.STORE_ADDR, FrameAddr);
            if (!ret.IsSuccessful)
            {
                logger.Error($"Failed to write STORE_ADDR to camera at {this.address}:{this.port}, error: {ret.Error}");
                return new(ret.Error);
            }
            if (!ret.Value)
            {
                logger.Error($"STORE_ADDR write returned false for camera at {this.address}:{this.port}");
                return new(new Exception($"STORE_ADDR write returned false for camera at {this.address}:{this.port}"));
            }
        }

        {
            var ret = await UDPClientPool.WriteAddr(this.ep, 2, CameraAddr.STORE_NUM, FrameLength);
            if (!ret.IsSuccessful)
            {
                logger.Error($"Failed to write STORE_NUM to camera at {this.address}:{this.port}, error: {ret.Error}");
                return new(ret.Error);
            }
            if (!ret.Value)
            {
                logger.Error($"STORE_NUM write returned false for camera at {this.address}:{this.port}");
                return new(new Exception($"STORE_NUM write returned false for camera at {this.address}:{this.port}"));
            }
        }

        var i2c = new Peripherals.I2cClient.I2c(this.address, this.port, this.timeout);

        foreach (var cmd in InitCmdData)
        {
            var ret = await i2c.WriteData(CAM_I2C_ADDR, cmd, CAM_PROTO);

            if (!ret.IsSuccessful)
            {
                logger.Error($"I2C write 0x{CAM_I2C_ADDR.ToString("X")} failed: {BitConverter.ToString(cmd)} error: {ret.Error}");
                return new(ret.Error);
            }

            if (!ret.Value)
            {
                logger.Error($"I2C write 0x{CAM_I2C_ADDR.ToString("X")} returned false: {BitConverter.ToString(cmd)}");
                return false;
            }
        }

        return true;
    }

    public async ValueTask<Result<bool>> EnableCamera(bool isEnable)
    {
        var ret = await UDPClientPool.WriteAddr(this.ep, 2, CameraAddr.CAPTURE_ON, Convert.ToUInt32(isEnable));
        if (!ret.IsSuccessful)
        {
            logger.Error($"Failed to write CAPTURE_ON to camera at {this.address}:{this.port}, error: {ret.Error}");
            return new(ret.Error);
        }
        if (!ret.Value)
        {
            logger.Error($"CAPTURE_ON write returned false for camera at {this.address}:{this.port}");
            return false;
        }
        return true;
    }

    /// <summary>
    /// 读取一帧图像数据
    /// </summary>
    /// <returns>包含图像数据的字节数组</returns>
    public async ValueTask<Result<byte[]>> ReadFrame()
    {
        // 清除UDP服务器接收缓冲区
        await MsgBus.UDPServer.ClearUDPData(this.address, 2);

        logger.Trace($"Clear up udp server {this.address} receive data");

        // 使用UDPClientPool读取图像帧数据
        var result = await UDPClientPool.ReadAddr4Bytes(
            this.ep,
            2, // taskID
            FrameAddr,
            ((int)FrameLength),
            this.timeout);

        if (!result.IsSuccessful)
        {
            logger.Error($"Failed to read frame from camera {this.address}:{this.port}, error: {result.Error}");
            return new(result.Error);
        }

        logger.Debug($"Successfully read frame from camera {this.address}:{this.port}, data length: {result.Value.Length} bytes");
        return result.Value;
    }
}