from machine import Pin, I2C
import utime
import struct

I2C_SLAVE_DEV_ADDR = 0x11
ECONF_REG_ADDR = 0x0B
CVAL_REG_ADDR = 0x03

# I2C init (SDA:0pin, SCL:1pin)
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=100000)

# Write 0x07 to ECONF(0x0B)
i2c.writeto_mem(I2C_SLAVE_DEV_ADDR, ECONF_REG_ADDR, b'\x07')

# Wait until the config is written and the device reboots
print("Rebooting...")
utime.sleep(4)

while True:
    utime.sleep(0.1)
    # Read 4 bytes from CVAL(0x03)
    data = i2c.readfrom_mem(I2C_SLAVE_DEV_ADDR, CVAL_REG_ADDR, 4)

    # Little-endian conversion to type int32
    value = struct.unpack('<i', data)[0]  # '<' means little-endian

    # Output result
    print("val:", value)

using System;
using System.IO.Ports;  // https://www.nuget.org/packages/System.IO.Ports
using System.Collections.Concurrent;

public class Counter
{
    public bool z_enable;
    public int count, last_z_count;
    public bool z_detected;

    public Counter()
    {
        z_enable = false;
        count = 0;
        last_z_count = 0;
        z_detected = false;
    }

    public void convertValue(string s)
    {
        string[] words = s.Split(':');
        switch (words.Length)
        {
            case 1:
                z_enable = false;
                try
                {
                    count = Int32.Parse(words[0]);
                }
                catch (FormatException)
                {
                    Console.WriteLine($"Unable to parse '{s}'");
                }
                break;
            case 3:
                z_enable = true;
                try
                {
                    count = Int32.Parse(words[0]);
                    last_z_count = Int32.Parse(words[1]);
                    z_detected = Int32.Parse(words[2]) == 1 ? true : false;
                }
                catch (FormatException)
                {
                    Console.WriteLine($"Unable to parse '{s}'");
                }
                break;
            default:
                Console.WriteLine($"Unable to parse '{s}'");
                break;
        }
    }
}

public class Obtain2EncoderCounterValue
{
    static bool _continue;
    static SerialPort? _serialPort1, _serialPort2;
    static ConcurrentQueue<Counter> _queue1 = new ConcurrentQueue<Counter>(), _queue2 = new ConcurrentQueue<Counter>();

    public static void Main()
    {
        string message;
        StringComparer stringComparer = StringComparer.OrdinalIgnoreCase;
        Thread readThread1 = new Thread(Read1);
        Thread readThread2 = new Thread(Read2);
        Thread mergeThread = new Thread(Merge);

        Console.WriteLine("Output synchronized values of 2 encoder counters\n");

        printAvailablePorts();
        Console.Write("First  encoder counter: ");
        _serialPort1 = new SerialPort(InputPortName(), 921600, Parity.None, 8, StopBits.One);
        Console.Write("Second encoder counter: ");
        _serialPort2 = new SerialPort(InputPortName(), 921600, Parity.None, 8, StopBits.One);

        _serialPort1.ReadTimeout = 500;
        _serialPort1.WriteTimeout = 500;
        _serialPort2.ReadTimeout = 500;
        _serialPort2.WriteTimeout = 500;

        Console.WriteLine("{0}\t{1}", _serialPort1.IsOpen, _serialPort2.IsOpen);
        _serialPort1.Open();
        _serialPort2.Open();
        _continue = true;
        readThread1.Start();
        readThread2.Start();
        mergeThread.Start();

        Console.WriteLine("Type QUIT to exit");

        while (_continue)
        {
            message = Console.ReadLine()!;

            if (stringComparer.Equals("quit", message))
            {
                _continue = false;
            }
        }

        readThread1.Join();
        readThread2.Join();
        mergeThread.Join();
        _serialPort1.Close();
        _serialPort2.Close();
    }

    public static void Read1()
    {
        while (_continue)
        {
            try
            {
                Counter c = new Counter();
                string message = _serialPort1!.ReadLine();
                c.convertValue(message);
                _queue1.Enqueue(c);
            }
            catch (TimeoutException) { }
        }
    }

    public static void Read2()
    {
        while (_continue)
        {
            try
            {
                Counter c = new Counter();
                string message = _serialPort2!.ReadLine();
                c.convertValue(message);
                _queue2.Enqueue(c);
            }
            catch (TimeoutException) { }
        }
    }

    public static void Merge()
    {
        int i = 0;
        while (_continue)
        {
            if(_queue1.Count() > 0 && _queue2.Count() > 0)
            {
                Counter c1, c2;
                if(_queue1.TryDequeue(out Counter? a))
                {
                    c1 = a!;
                } else
                {
                    continue;
                }

                if(_queue2.TryDequeue(out Counter? b))
                {
                    c2 = b!;
                } else
                {
                    continue;
                }
                string text;
                if(c1.z_enable && c2.z_enable)
                    text = $"{i++}\tc1:{c1.count},{c1.last_z_count},{c1.z_detected}\tc2:{c2.count},{c2.last_z_count},{c2.z_detected}";
                else if(c1.z_enable && !c2.z_enable)
                    text = $"{i++}\tc1:{c1.count},{c1.last_z_count},{c1.z_detected}\tc2:{c2.count}";
                else if(!c1.z_enable && c2.z_enable)
                    text = $"{i++}\tc1:{c1.count}\tc2:{c2.count},{c2.last_z_count},{c2.z_detected}";
                else
                    text = $"{i++}\tc1:{c1.count}\tc2:{c2.count}";
                Console.WriteLine(text);
                File.AppendAllText(@"./output.txt", text.Replace(",", "\t").Replace("c1:", "").Replace("c2:", "") + '\n');
            }
        }
    }

    public static void printAvailablePorts()
    {
        Console.WriteLine("Available Ports:");
        foreach (string s in SerialPort.GetPortNames())
        {
            Console.WriteLine("\t{0}", s);
        }
    }

    public static string InputPortName()
    {
        string portName;

        string[] ports = SerialPort.GetPortNames();
        string defaultPortName = ports.Length >= 2 ? ports[^2] : "COM1";
        Console.Write("Enter COM port value (Default: {0}): ", defaultPortName);
        portName = Console.ReadLine()!;

        if (portName == "" || !(portName.ToLower()).StartsWith("com"))
        {
            portName = defaultPortName;
        }
        return portName;
    }
}