using CosmicDriver;

var sync = new object();
int gpioInterruptCount = 0;
bool gpioInterruptInvalid = false;

using var sdk = new CosmicSdk(2000);
if (!sdk.Connect()) throw new Exception("Failed to connect to a Binho device.");

sdk.OnNotification(notification =>
{
    if (notification.Type != NotificationType.NOTIF_GPIO || notification.Gpio is null)
    {
        return;
    }

    int gpioPinLabel = notification.Gpio.PinNumber + 1;
    Console.WriteLine($"GPIO interrupt received on pin {gpioPinLabel}");
    if (notification.Gpio.PinNumber != (byte)GpioPinNumber.GPIO_1)
    {
        gpioInterruptInvalid = true;
    }

    lock (sync)
    {
        gpioInterruptCount += 1;
        Monitor.PulseAll(sync);
    }
});

var (status, info) = sdk.GetDeviceInfo();
AssertStatus(status, "GetDeviceInfo");
PrintDeviceInfo(info!);

AssertStatus(sdk.GpioSetVoltage(1800), "GpioSetVoltage");

AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_1, GpioFunctionality.GPIO_DIGITAL_INPUT), "GpioConfigurePin(GPIO_1, INPUT)");
AssertStatus(sdk.GpioConfigureInterrupt(GpioPinNumber.GPIO_1, GpioTriggerType.GPIO_TRIGGER_FALLING_EDGE), "GpioConfigureInterrupt(GPIO_1, FALLING)");
AssertStatus(sdk.GpioDisableInterrupt(GpioPinNumber.GPIO_1), "GpioDisableInterrupt(GPIO_1)");

AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_3, GpioFunctionality.GPIO_DIGITAL_INPUT), "GpioConfigurePin(GPIO_3, INPUT)");
AssertStatus(sdk.GpioConfigureInterrupt(GpioPinNumber.GPIO_3, GpioTriggerType.GPIO_TRIGGER_RISING_EDGE), "GpioConfigureInterrupt(GPIO_3, RISING)");
AssertStatus(sdk.GpioDisableInterrupt(GpioPinNumber.GPIO_3), "GpioDisableInterrupt(GPIO_3)");

AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_5, GpioFunctionality.GPIO_DIGITAL_INPUT), "GpioConfigurePin(GPIO_5, INPUT)");
AssertStatus(sdk.GpioConfigureInterrupt(GpioPinNumber.GPIO_5, GpioTriggerType.GPIO_TRIGGER_BOTH_EDGES), "GpioConfigureInterrupt(GPIO_5, BOTH)");
AssertStatus(sdk.GpioDisableInterrupt(GpioPinNumber.GPIO_5), "GpioDisableInterrupt(GPIO_5)");
AssertStatus(sdk.GpioDisableInterrupt(GpioPinNumber.GPIO_5), "GpioDisableInterrupt(GPIO_5) again");

AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_2, GpioFunctionality.GPIO_DIGITAL_OUTPUT), "GpioConfigurePin(GPIO_2, OUTPUT)");
ExpectStatus(sdk.GpioConfigureInterrupt(GpioPinNumber.GPIO_2, GpioTriggerType.GPIO_TRIGGER_BOTH_EDGES), "GpioConfigureInterrupt(GPIO_2, BOTH)", ResultCode.FW_GPIO_WRONG_CONFIGURATION);
ExpectStatus(sdk.GpioDisableInterrupt(GpioPinNumber.GPIO_2), "GpioDisableInterrupt(GPIO_2)", ResultCode.FW_GPIO_WRONG_CONFIGURATION);

AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_4, GpioFunctionality.GPIO_DIGITAL_INPUT), "GpioConfigurePin(GPIO_4, INPUT)");
AssertStatus(sdk.GpioConfigureInterrupt(GpioPinNumber.GPIO_4, GpioTriggerType.GPIO_TRIGGER_BOTH_EDGES), "GpioConfigureInterrupt(GPIO_4, BOTH)");
AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_4, GpioFunctionality.GPIO_DIGITAL_OUTPUT), "GpioConfigurePin(GPIO_4, OUTPUT)");
ExpectStatus(sdk.GpioDisableInterrupt(GpioPinNumber.GPIO_4), "GpioDisableInterrupt(GPIO_4)", ResultCode.FW_GPIO_WRONG_CONFIGURATION);

Console.WriteLine("Assuming GPIO 1 is short-circuited with GPIO 3");
AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_1, GpioFunctionality.GPIO_DIGITAL_INPUT), "GpioConfigurePin(GPIO_1, INPUT for echo)");
AssertStatus(sdk.GpioConfigurePin(GpioPinNumber.GPIO_3, GpioFunctionality.GPIO_DIGITAL_OUTPUT, GpioLogicLevel.GPIO_LOW), "GpioConfigurePin(GPIO_3, OUTPUT for echo)");

AssertStatus(sdk.GpioDigitalWrite(GpioPinNumber.GPIO_3, GpioLogicLevel.GPIO_HIGH), "GpioDigitalWrite(GPIO_3, HIGH)");
var (readHighStatus, levelHigh) = sdk.GpioDigitalRead(GpioPinNumber.GPIO_1);
AssertStatus(readHighStatus, "GpioDigitalRead(GPIO_1)");
if (levelHigh != GpioLogicLevel.GPIO_HIGH) throw new Exception("GPIO_1 did not read HIGH as expected.");

AssertStatus(sdk.GpioDigitalWrite(GpioPinNumber.GPIO_3, GpioLogicLevel.GPIO_LOW), "GpioDigitalWrite(GPIO_3, LOW)");
var (readLowStatus, levelLow) = sdk.GpioDigitalRead(GpioPinNumber.GPIO_1);
AssertStatus(readLowStatus, "GpioDigitalRead(GPIO_1)");
if (levelLow != GpioLogicLevel.GPIO_LOW) throw new Exception("GPIO_1 did not read LOW as expected.");

AssertStatus(sdk.GpioConfigureInterrupt(GpioPinNumber.GPIO_1, GpioTriggerType.GPIO_TRIGGER_BOTH_EDGES), "GpioConfigureInterrupt(GPIO_1, BOTH)");

const int maxInterrupts = 5;
int lastInterruptCount = gpioInterruptCount;
while (lastInterruptCount < maxInterrupts)
{
    AssertStatus(sdk.GpioDigitalWrite(GpioPinNumber.GPIO_3, GpioLogicLevel.GPIO_HIGH), "GpioDigitalWrite(GPIO_3, HIGH)");
    Thread.Sleep(1000);
    ValidateInterruptProgress(lastInterruptCount, "after setting GPIO_3 HIGH");
    lastInterruptCount = gpioInterruptCount;
    if (lastInterruptCount >= maxInterrupts)
    {
        break;
    }

    AssertStatus(sdk.GpioDigitalWrite(GpioPinNumber.GPIO_3, GpioLogicLevel.GPIO_LOW), "GpioDigitalWrite(GPIO_3, LOW)");
    Thread.Sleep(1000);
    ValidateInterruptProgress(lastInterruptCount, "after setting GPIO_3 LOW");
    lastInterruptCount = gpioInterruptCount;
}

Console.WriteLine($"Received {lastInterruptCount} GPIO interrupts");

void ValidateInterruptProgress(int previousCount, string stage)
{
    if (gpioInterruptInvalid)
    {
        throw new Exception($"Unexpected GPIO interrupt pin reported {stage}.");
    }
    if (gpioInterruptCount == previousCount)
    {
        throw new Exception($"No GPIO interrupt received {stage}.");
    }
}

static void PrintDeviceInfo(DeviceInfo info)
{
    Console.WriteLine($"Manufacturer: {info.Manufacturer}");
    Console.WriteLine($"Product name: {info.ProductName}");
    Console.WriteLine($"Serial number: {info.SerialNumber}");
    Console.WriteLine($"FW version: {info.FwVersion}");
    Console.WriteLine($"HW version: {info.HwVersion}");
}

static void ExpectStatus(ResultCode status, string operation, ResultCode expected)
{
    if (status != expected)
    {
        throw new Exception($"{operation} expected STATUS=0x{(ushort)expected:X4} but got 0x{(ushort)status:X4}");
    }
}

static void AssertStatus(ResultCode status, string operation, params ResultCode[] expected)
{
    if (expected.Length == 0) expected = [ResultCode.SUCCESS];
    if (!expected.Contains(status)) throw new Exception($"{operation} failed. STATUS=0x{(ushort)status:X4}");
}
