using System.CommandLine;

using GSS2.Core;
using GSS2.Core.Hardware;
using GSS2.Core.Logging;

using Microsoft.Extensions.Logging.Console;

using OpenCvSharp;

using ConsoleFormatter = GSS2.Core.Logging.ConsoleFormatter;

namespace GSS2.Test;

public partial class Program
{
    private class CameraTuningWorker : BackgroundService
    {
        private readonly ILogger<CameraTuningWorker> _logger;
        private readonly IHostApplicationLifetime _applicationLifetime;
        private readonly CameraService _cameraService;
        private readonly IlluminatorService _illuminatorService;
        private readonly FileInfo? _outputFile;
        private double _gainR;
        private double _gainG;
        private double _gainB;
        private readonly double _threshold;
        private readonly uint _steps;
        private static readonly Scalar[] KnownSegments =
        {
            new Scalar(0,255,255),   // Cyan
            new Scalar(0,0,255),     // Blue
            new Scalar(255,0,255),   // Magenta
            new Scalar(128,128,128), // Gray (skip)
            new Scalar(255,0,0),     // Red
            new Scalar(255,128,0),   // Orange
            new Scalar(255,255,0),   // Yellow
            new Scalar(0,255,0)      // Green
        };
        private const double PatternOuterRadius = 180.0;
        private const double PatternCenterRadius = 60.0;

        public CameraTuningWorker(ILogger<CameraTuningWorker> logger, IHostApplicationLifetime applicationLifetime, CameraService cameraService, IlluminatorService illuminatorService, FileInfo? outputFile, double gainR, double gainG, double gainB, double threshold, uint steps)
        {
            _applicationLifetime = applicationLifetime;
            _logger = logger;
            _cameraService = cameraService;
            _illuminatorService = illuminatorService;
            _outputFile = outputFile;
            _gainR = gainR;
            _gainG = gainG;
            _gainB = gainB;
            _threshold = threshold;
            _steps = steps;
            _logger.LogInformation("Initialized");
            _logger.LogInformation("Output File: {}", _outputFile);
            _logger.LogInformation("Gain R: {}", _gainR);
            _logger.LogInformation("Gain G: {}", _gainG);
            _logger.LogInformation("Gain B: {}", _gainB);
            _logger.LogInformation("Threshold: {}", _threshold);
            _logger.LogInformation("Steps: {}", _steps);
        }

        protected override async Task ExecuteAsync(CancellationToken stoppingToken)
        {
            _logger.LogInformation("Running camera fine tuning");

            Mat? image;

            int i = 0;
            bool optimalGainsFound = false;

            _illuminatorService.SetIntensity(1, 0, 0);
            _illuminatorService.TurnOn();

            while (i < _steps)
            {
                _cameraService.Configuration.DecodeGainR = _gainR;
                _cameraService.Configuration.DecodeGainG = _gainG;
                _cameraService.Configuration.DecodeGainB = _gainB;

                try
                {
                    image = await _cameraService.CaptureImage(stoppingToken, 2);

                    if (image is null)
                    {
                        _logger.LogError("Cannot get image");
                        break;
                    }

                    if (image.Type() != MatType.CV_8UC3 &&
                        image.Type() != MatType.CV_8UC4 &&
                        image.Type() != MatType.CV_16UC3 &&
                        image.Type() != MatType.CV_16UC4)
                    {
                        _logger.LogCritical("Unsupported image type: {}", image.Type().ToString());
                        break;
                    }

                    if (image.Type() == MatType.CV_16UC3)
                        image.ConvertTo(image, MatType.CV_8UC3, 1 / 256.0);
                    if (image.Type() == MatType.CV_16UC4)
                        image.ConvertTo(image, MatType.CV_8UC4, 1 / 256.0);

                    var channels = image.Split();
                    Cv2.Merge([channels[0], channels[1], channels[2]], image);
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Exception occurred while image capturing");
                    break;
                }

                try
                {
                    if (_outputFile is not null)
                        image.ImWrite(_outputFile.FullName);
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Exception occurred while image saving");
                }

                try
                {
                    var gains = ComputeWhiteBalanceGains(image);
                    if (gains is null)
                    {
                        _logger.LogError("Cannot compute gains");
                        break;
                    }

                    var (r, g, b) = gains.Value;
                    if (Math.Abs(_gainR - r) < _threshold &&
                        Math.Abs(_gainG - g) < _threshold &&
                        Math.Abs(_gainB - b) < _threshold)
                    {
                        _logger.LogInformation("Optimal color gains: r={}, g={}, b:={}", r, g, b);
                        optimalGainsFound = true;
                        break;
                    }
                    _gainR = (_gainR + r) / 2;
                    _gainG = (_gainG + g) / 2;
                    _gainB = (_gainB + b) / 2;
                    _logger.LogInformation("Step {}/{} color gains: r={}, g={}, b:={}", i + 1, _steps, _gainR, _gainG, _gainB);
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Exception occurred while gains computing");
                    break;
                }
                i++;
            }
            _illuminatorService.TurnOff();

            if (!optimalGainsFound)
                _logger.LogWarning("Optimal gains not found");

            _applicationLifetime.StopApplication();
        }

        private (double rGain, double gGain, double bGain)? ComputeWhiteBalanceGains(Mat image)
        {
            var circle = FindCircle(image);
            if (circle == null)
                return null;

            var (rx, ry, rr) = circle.Value;

            double scale = rr / PatternOuterRadius;
            double innerPx = PatternCenterRadius * scale;
            int midR = (int)((innerPx + rr) / 2.0);

            double phi0 = DetectOrientation(image, rx, ry, rr);
            if (double.IsNaN(phi0))
                phi0 = Math.PI / 2.0;

            var samples = new List<Vec3d>();
            var refs = new List<Vec3d>();

            int N = KnownSegments.Length;

            for (int i = 0; i < N; i++)
            {
                if (i == 3) continue; // skip gray

                double theta = phi0 - (2 * Math.PI / N) * i;

                int cx = (int)(rx + midR * Math.Cos(theta));
                int cy = (int)(ry - midR * Math.Sin(theta));

                var roi = new Rect(
                    Math.Max(cx - 8, 0),
                    Math.Max(cy - 8, 0),
                    Math.Min(16, image.Width - Math.Max(cx - 8, 0)),
                    Math.Min(16, image.Height - Math.Max(cy - 8, 0)));

                if (roi.Width < 8 || roi.Height < 8)
                    continue;

                using var patch = new Mat(image, roi);
                var mean = Cv2.Mean(patch);

                // BGR → RGB
                samples.Add(new Vec3d(mean.Val2, mean.Val1, mean.Val0));

                var refColor = KnownSegments[i];
                refs.Add(new Vec3d(refColor.Val2, refColor.Val1, refColor.Val0));
            }

            if (samples.Count < 3)
                return null;

            // Формируем матрицы для least squares
            var S = new Mat(samples.Count, 3, MatType.CV_64F);
            var R = new Mat(samples.Count, 3, MatType.CV_64F);

            for (int i = 0; i < samples.Count; i++)
            {
                S.Set(i, 0, samples[i][0]);
                S.Set(i, 1, samples[i][1]);
                S.Set(i, 2, samples[i][2]);

                R.Set(i, 0, refs[i][0]);
                R.Set(i, 1, refs[i][1]);
                R.Set(i, 2, refs[i][2]);
            }

            var M = new Mat();
            Cv2.Solve(S, R, M, DecompTypes.Normal);

            double rGain = Math.Max(M.At<double>(0, 0), 1.0);
            double gGain = 1.0;
            double bGain = Math.Max(M.At<double>(2, 2), 1.0);

            return (rGain, gGain, bGain);
        }
        private (int x, int y, int r)? FindCircle(Mat image)
        {
            using var gray = new Mat();
            Cv2.CvtColor(image, gray, ColorConversionCodes.BGR2GRAY);
            Cv2.GaussianBlur(gray, gray, new Size(9, 9), 2);

            var circles = Cv2.HoughCircles(
                gray,
                HoughModes.Gradient,
                dp: 1.2,
                minDist: gray.Rows / 3,
                param1: 100,
                param2: 30,
                minRadius: (int)(gray.Rows * 0.3),
                maxRadius: (int)(gray.Rows * 0.6));

            if (circles.Length == 0)
                return null;

            var c = circles[0];
            return ((int)c.Center.X, (int)c.Center.Y, (int)c.Radius);
        }
        private double DetectOrientation(Mat image, int rx, int ry, int rr)
        {
            double scale = rr / PatternOuterRadius;
            int innerR = (int)(PatternCenterRadius * scale);

            using var gray = new Mat();
            Cv2.CvtColor(image, gray, ColorConversionCodes.BGR2GRAY);

            using var mask = Mat.Zeros(image.Size(), MatType.CV_8U).ToMat();
            Cv2.Circle(mask, new Point(rx, ry), innerR, Scalar.White, -1);

            var whitePoints = new List<Point>();

            for (int y = 0; y < gray.Rows; y++)
            {
                for (int x = 0; x < gray.Cols; x++)
                {
                    if (mask.At<byte>(y, x) == 255 &&
                        gray.At<byte>(y, x) > 128)
                    {
                        whitePoints.Add(new Point(x, y));
                    }
                }
            }

            if (whitePoints.Count < 20)
                return double.NaN;

            double cx = whitePoints.Average(p => p.X);
            double cy = whitePoints.Average(p => p.Y);

            return Math.Atan2(ry - cy, cx - rx);
        }
    }
    private static void CameraTuning(ParseResult parseResult)
    {
        var outputFile = parseResult.GetValue(_cameraTuningOutputFile);
        var gainR = parseResult.GetRequiredValue(_cameraTuningGainR);
        var gainG = parseResult.GetRequiredValue(_cameraTuningGainG);
        var gainB = parseResult.GetRequiredValue(_cameraTuningGainB);
        var threshold = parseResult.GetRequiredValue(_cameraTuningThreshold);
        var steps = parseResult.GetRequiredValue(_cameraTuningSteps);

        var builder = Host.CreateApplicationBuilder();

        builder.Logging.ClearProviders();
        builder.Logging.AddConsole(options => options.FormatterName = nameof(ConsoleFormatter));
        builder.Logging.AddConsoleFormatter<ConsoleFormatter, ConsoleFormatterOptions>();
        builder.Logging.AddProvider(new FileLoggerProvider("full.log", true));
        builder.Logging.AddProvider(new FileLoggerProvider("last_run.log", false));
        builder.Services.AddSingleton<LibCameraLogSink>();

        builder.Services.AddIlluminatorService("Hardware:Illuminator");
        builder.Services.AddCameraService("Hardware:Camera", true);

        builder.Services.AddHostedService<CameraTuningWorker>(services =>
        {
            var logger = services.GetRequiredService<ILogger<CameraTuningWorker>>();
            var applicationLifetime = services.GetRequiredService<IHostApplicationLifetime>();
            var cameraService = services.GetRequiredService<CameraService>();
            var illuminatorService = services.GetRequiredService<IlluminatorService>();
            return new CameraTuningWorker(logger, applicationLifetime, cameraService, illuminatorService, outputFile, gainB, gainG, gainR, threshold, steps);
        });

        var host = builder.Build();
        host.Services.GetRequiredService<LibCameraLogSink>();

        host.Run();
    }
}