#include "Lgi.h"
#include "Hardware.h"
#include "GVariant.h"
#include "mc4.h"
#include "../rotary/rotary.h"

#include <errno.h>

#define DEBUG_SHOW_BTN_BITS		0
#define DEBUG_LED_NON_BLACK		0
#define DEBUG_HARDWARE_EVENTS	0
#define DEBUG_ANALOG_INPUTS     0

#define MAX_LED_BRIGHTNESS		30
#define HW_BUTTONS				14
#define LED_COUNT				14
#define ENC_BUTTONS				6
#define ENCODERS				6
#define ANALOG_INPUTS			2
#define SECOND					1000
#define MIDI_TIMEOUT			(3*SECOND)
#define MIN_BUTTON_EVENT_MS		200
#define HOLD_BUTTON_EVENT_MS	1200
#define ENCODER_MIN				0
#define ENCODER_MAX				127
#define ENCODER_ACCEL_THRESHOLD 80
#define ENCODER_ACCEL_RATE		8

// LED Output
	#include "clk.h"
	#include "gpio.h"
	#include "dma.h"
	#include "pwm.h"
	#include "ws2811.h"

	#define TARGET_FREQ			WS2811_TARGET_FREQ
	#define GPIO_PIN			18
	#define WS2811_DMA			5
	#define LED_COUNT			(2 * 7)

// Midi I/O ports
	#include <sys/select.h>
	#include <asm/termbits.h>
	// sudo chmod a+rw /dev/ttyAMA0

// I2C bus
	#include <unistd.h>			// open, close
	#include <fcntl.h>			// O_RDWR, O_SYNC
	#include <sys/mman.h>		// mmap
	#include <linux/i2c-dev.h>	// I2C_SLAVE?
	#include <sys/ioctl.h>

	#define BCM2708_PERI_BASE	0x20000000
	#define BLOCK_SIZE			4096
	#define GPIO_BASE			(BCM2708_PERI_BASE + 0x00200000)
	#define I2C_DEVICE			"/dev/i2c-0"	// I2C bus

// MCP23017 chips (on I2C bus)

	// Default is IOCON.BANK = 0
	// i2cdetect -y 0

	// Chip addresses
	#define MCP23017_ADDR1		0x20
	#define MCP23017_ADDR2		0x21

	// Registers on each chip (for IOCON.BANK = 0)
	#define MCP23017_IODIRA		0x00
	#define MCP23017_IODIRB		0x01
	#define MCP23017_IPOLA		0x02
	#define MCP23017_IPOLB		0x03
	#define MCP23017_GPINTENA	0x04
	#define MCP23017_GPINTENB	0x05
	#define MCP23017_DEFVALA	0x06
	#define MCP23017_DEFVALB	0x07
	#define MCP23017_INTCONA	0x08
	#define MCP23017_INTCONB	0x09
	#define MCP23017_IOCONA		0x0A
	#define MCP23017_IOCONB		0x0B
	#define MCP23017_GPPUA		0x0C
	#define MCP23017_GPPUB		0x0D
	#define MCP23017_INTFA		0x0E
	#define MCP23017_INTFB		0x0F
	#define MCP23017_INTCAPA	0x10
	#define MCP23017_INTCAPB	0x11
	#define MCP23017_GPIOA		0x12
	#define MCP23017_GPIOB		0x13
	#define MCP23017_OLATA		0x14
	#define MCP23017_OLATB		0x15

// Analog inputs: ADS1115
	#include "ADS1115.h"
	#define ADC_THRESHOLD		10
	#define ADC_REPLUG_MS		250
	#define ADC_UNPLUGGED		15000

// I2C to SPI bridge
    #define SC18IS602B_ADDR     0x28
    #define DEBUG_USE_GPIO		0
    typedef char LedRingBits[6];
    GRgb24 LedRingAddr[16] =
    {
        {26,28,30}, // 0 - down
        {29,31,32}, // 1
        {38,36,34}, // 2
        {37,35,33}, // 3 - left
        {40,39,42}, // 4
        {43,44,46}, // 5
        {41,45,47}, // 6
        {9, 5, 7 }, // 7
        {15,13,11}, // 8 - up
        {12,10,8 }, // 9
        {1, 3, 14}, // 10
        {6, 2, 4 }, // 11
        {22,20,0 }, // 12
        {21,16,18}, // 13 - right
        {17,19,23}, // 14
        {24,27,25}, // 15
    };
    #define LedTestColour GColour(0, 0, 1)
    #define DefaultLedRingColour GColour(0, 0, 1)
    #define LED_OUTPUT_DISABLE	0x08    
    enum SpiClockRate
    {
        SPI_1843_kHz,
        SPI_461_kHz,
        SPI_115_kHz,
        SPI_58_kHz
    };
    enum SpiGpioMode
    {
        SPI_QuasiBidirectional,
        SPI_PushPull,
        SPI_InputOnly,
        SPI_OpenDrain,
    };


// Mapping between hardware button address and logical address
int HwButtonMasks[] =
{
	// Top tow
	0x80, // Btn 0
	0x40, // Btn 1
	0x20, // Btn 2
	0x10, // Btn 3
	0x8, // Btn 4
	0x4, // Btn 5
	0x2, // Btn 6

	// Bottom row
	0x1, // Btn 7
	0x800000, // Btn 8
	0x400000, // Btn 9
	0x200000, // Btn 10
	0x100000, // Btn 11
	0x80000, // Btn 12
	0x40000, // Btn 13

	// Encoder buttons
	0x1000,
	0x2000,
	0x4000,
	0x8000,
	0x20000,
	0x10000
};

// Shifts for the encoder quadrature locations
int EncoderShifts[] =
{
	8,	// 0x00000300
	10, // 0x00000c00
	24, // 0x03000000
	26, // 0x0c000000
	28, // 0x30000000
	30	// 0xc0000000
};

bool InitSecondaryI2CBus()
{
	int fileHandle = 0;

	if ((fileHandle = open("/dev/mem", O_RDWR|O_SYNC)) < 0)
	{
		printf("%s:%i - Failed to open /dev/mem, try checking permissions.\n", _FL);
		return false;
	}

	void *map = mmap
	(
		0,
		BLOCK_SIZE,
		PROT_READ|PROT_WRITE,
		MAP_SHARED,
		fileHandle,
		GPIO_BASE
	);

	if (map == MAP_FAILED)
	{
		printf("%s:%i - Failed to mem-map /dev/mem.\n", _FL);
		return false;
	}

	volatile unsigned int *addr = (volatile unsigned int *)map;

	int reg0 = addr[0];
	int m0 = 0b00000000000000000000111111111111;
	int s0 = 0b00000000000000000000100100000000;
	int b0 = reg0 & m0;
	if (b0 != s0)
	{
		//	need to change register 0
		addr[0] = (reg0 & ~m0) | s0;
		printf("\tChanging reg 0.\n");
	}
	else
	{
		// register 0 is ok
		printf("\treg 0 ok.\n");
	}

	int reg2 = addr[2];
	int m2 = 0b00111111000000000000000000000000;
	int s2 = 0b00100100000000000000000000000000;
	int b2 = reg2 & m2;
	if (b2 != s2)
	{
		// need to change register 2
		addr[2] = (reg2 & ~m2) | s2;
		printf("\tChanging reg 2.\n");
	}
	else
	{
		// register 2 is ok
		printf("\treg 2 ok.\n");
	}

	munmap(map, BLOCK_SIZE);
	close(fileHandle);
}

class EncRing : public Rotary
{
    int Value;
    uint64 Ts;

public:
    int Led;
    GColour Col;
    bool Dirty;

    int GetValue()
    {
        return Value;
    }

    void SetValue(int v)
    {
        Value = limit(v, 0, 127);
    }
    
    bool Update(int Bits, uint64 Now, bool Debug = false)
    {
		uint64 Diff = Now - Ts;
		int Rate =	Diff < ENCODER_ACCEL_THRESHOLD ?
					(ENCODER_ACCEL_THRESHOLD - Diff) / ENCODER_ACCEL_RATE :
					1;

        StepType s = process(Bits);
        if (s == DIR_NONE)
            return false;

        Ts = Now;
        if (s == DIR_CW)
        {
            if (Value < 127)
            {
                int n = MIN(Value + Rate, 127);
                if (Value != n)
                {
                    Value = n;
                    
                    if (Debug)
                        printf("Enc +%i = %i (%i ms)\n", Rate, Value, (int)Diff);
                    
                    return true;
                }
            }
        }
        else
        {
            if (Value > 0)
            {
                int n = MAX(Value - Rate, 0);
                if (Value != n)
                {
                    Value = n;

                    if (Debug)
                        printf("Enc -%i = %i (%i ms)\n", Rate, Value, (int)Diff);

                    return true;
                }
            }
        }
        
        return false;
    }
};

struct HardwarePriv :
	public LThread,
	public GEventSinkI
{
	Hardware *Hw;
	HardwareEvents *Callback;
	LMutex Lck;
	GArray<GMessage*> MsgQue;
	uint64 TimerTs;
	uint64 LedRingUpdateTs;

	HardwarePriv(Hardware *hw, HardwareEvents *callback) :
		Hw(hw),
		Callback(callback),
		LThread("HardwareThread"),
		Lck("HardwareMutex")
	{
		I2cHnd = -1;
		MidiHnd = -1;
		I2cAddr = -1;

		Encoders.Length(6);
		for (unsigned i=0; i<Encoders.Length(); i++)
		{
		    Encoders[i].Col = DefaultLedRingColour;
		}

		Pedals.Length(4);
		MidiTs = 0;
		MidiConnected = false;
		TimerTs = 0;
		LedRingUpdateTs = 0;
		DebugEnc = -1;
		ZeroObj(BtnTs);

        // Check led ring bits
        bool Used[48];
        ZeroObj(Used);
        GRgb24 *p = LedRingAddr;
        for (unsigned i=0; i<16; i++)
        {
            if (Used[p[i].r])
                printf("Error: Red bit %i is already used (i=%i).\n", p[i].r, i);
            else
                Used[p[i].r] = true;

            if (Used[p[i].g])
                printf("Error: Green bit %i is already used (i=%i).\n", p[i].g, i);
            else
                Used[p[i].g] = true;

            if (Used[p[i].b])
                printf("Error: Blue bit %i is already used (i=%i).\n", p[i].b, i);
            else
                Used[p[i].b] = true;
        }
        for (unsigned i=0; i<48; i++)
        {
            if (!Used[i])
                printf("Error: Bit %i not used.\n", i);
        }

		// Setup the MIDI input / output port
		printf("HardwarePriv: Opening MIDI device...\n");
		MidiHnd = open("/dev/ttyAMA0", O_RDWR | O_NOCTTY | O_NDELAY);
		if (MidiHnd < 0)
		{
			printf("%s:%i - open('/dev/ttyAMA0') failed.\n", _FL);
		}
		else
		{
			struct termios2 tio;
			ioctl(MidiHnd, TCGETS2, &tio);
			
			// tio.c_cflag &= ~CBAUD;
			tio.c_cflag = BOTHER | CS8 | CLOCAL | CREAD;
			tio.c_iflag = IGNPAR;
			tio.c_oflag = 0;
			tio.c_lflag = 0;
			tio.c_ispeed = 31250;
			tio.c_ospeed = 31250;

			ioctl(MidiHnd, TCSETS2, &tio);
		}

		// Setup all the LEDs
		Leds.freq = TARGET_FREQ;
		Leds.dmanum = WS2811_DMA;
		Leds.channel[0].gpionum = GPIO_PIN;
		Leds.channel[0].count = LED_COUNT;
		Leds.channel[0].invert = 0;
		Leds.channel[0].brightness = MAX_LED_BRIGHTNESS;
		Leds.channel[1].gpionum = 0;
		Leds.channel[1].count = 0;
		Leds.channel[1].invert = 0;
		Leds.channel[1].brightness = 0;
		
		printf("HardwarePriv: Opening ws2811 LEDs...\n");
		if (ws2811_init(&Leds))
		{
			printf("%s:%i - ws2811_init failed.\n", _FL);
		}
		
		// Set default colours
		auto Pixels = Px();
		if (Pixels)
		{
			for (unsigned i=0; i<LED_COUNT; i++)
			{
				GBgrx32 *p = Pixels + i;
				#if DEBUG_LED_NON_BLACK
				p->r = 32;
				p->g = 16;
				p->b = 0;
				#else
				p->r = 0;
				p->g = 0;
				p->b = 0;
				#endif
			}
		}

		Update();

		// Start up the I2C stuff
		printf("Calling InitSecondaryI2CBus...\n");
		InitSecondaryI2CBus();
		
		printf("Opening I2C bus...\n");
		I2cHnd = open(I2C_DEVICE, O_RDWR);
		if (I2cHnd < 0)
		{
			printf("%s:%i - Can't open I2C bus.\n", _FL);
		}
		else
		{
			if
			(
				!SetupMCP23017(MCP23017_ADDR1)
				#ifdef MCP23017_ADDR2
				||
				!SetupMCP23017(MCP23017_ADDR2)
				#endif
			)
			{
				printf("%s:%i - I2C setup failed.\n", _FL);
				close(I2cHnd);
				I2cHnd = -1;
			}

            // Do setup			
		    uint8_t MsbOrder = 0, Mode = 0, SpiClockRate = SPI_1843_kHz;
			WriteI2C(SC18IS602B_ADDR,
			         0xf0,
			         (uint8_t)
			         (
			            (MsbOrder << 5) |
			            (Mode << 2) |
			            (SpiClockRate)
			        ));

            // Enable GPIO on SS3
			WriteI2C(SC18IS602B_ADDR,
			         0xf6, // GPIO enable
			         (uint8_t) 0x08); // SS3 is GPIO (output enable)
			         
		    // Set mode on SS3
			WriteI2C(SC18IS602B_ADDR,
			         0xf7, // GPIO config
			         (uint8_t) (SPI_PushPull << 6) ); // Push pull mode for SS3

            // Write a test byte
            UpdateLedRing(  1,
                            Encoders[4].Led,
                            Encoders[4].Col,
                            Encoders[5].Led,
                            Encoders[5].Col);
            UpdateLedRing(  2,
                            Encoders[2].Led,
                            Encoders[2].Col,
                            Encoders[3].Led,
                            Encoders[3].Col);
		}

		// Run the thread for the receive side / MIDI input, and I2C stuff
		if (MidiHnd >= 0)
			Run();
	}

	~HardwarePriv()
	{
		if (I2cHnd >= 0)
		{
			printf("Closing I2C device...\n");
			close(I2cHnd);
			I2cHnd = -1;
		}
		
		if (MidiHnd >= 0)
		{
			printf("~HardwarePriv: Closing MIDI device...\n");
			close(MidiHnd);
			MidiHnd = -1;
			
			while (!IsExited())
				LgiSleep(10);

			printf("~HardwarePriv: MIDI input thread shutdown...\n");
		}

		printf("~HardwarePriv: Closing ws2811 LEDs...\n");
		ws2811_fini(&Leds);
	}

	bool PostEvent(int Cmd, GMessage::Param a = 0, GMessage::Param b = 0)
	{
		if (!Lck.Lock(_FL))
		{
			printf("%s:%i - Error: lock failed.\n", _FL);
			return false;
		}
		
		MsgQue.Add(new GMessage(Cmd, a, b));
		// printf("Adding msgque %i,%i,%i\n", Cmd, a, b);
		Lck.Unlock();
		
		return true;
	}

private:
	// Buttons
	GArray<Hardware::ButtonModes> BtnModes;
	uint64 BtnTs[HW_BUTTONS+ENC_BUTTONS];
	
	// LEDs
	ws2811_t Leds;
	
	// MIDI input / output
	int MidiHnd;
	uint64 MidiTs;
	bool MidiConnected;
	GArray<uint8_t> MidiIn;

	// I2C bus
	int I2cHnd;
	int I2cAddr;
	
	// Continuous parameters
	GArray<EncRing> Encoders;
	GArray<int> Pedals;
	int DebugEnc;

	bool ClearIntLedRing()
	{
		// Set SS3 to low (output enabled)
		int i = 0;
		bool b;
		
		// Loop over this write until it succeeds.
		while (i < 100)
		{
			b = WriteI2C(	SC18IS602B_ADDR,
							0xf1, // Clear interupt
							NULL,
							0,
							true);
			if (b)
				break;
			i++;
		}

		if (!b)
			printf("%s:%i - ClearIntLedRing failed after %i interations.\n", _FL, i);
		return b;
	}

    bool EnableLedRing(bool e)
    {
		// Set SS3 to low (output enabled)
		bool b = WriteI2C(SC18IS602B_ADDR,
						0xf4, // GPIO write
						(uint8_t) (e ? 0 : LED_OUTPUT_DISABLE));
		if (!b)
			printf("%s:%i - EnableLedRing(%i) failed\n", _FL, e);
		return b;
    }

    // Methods
	bool UpdateLedRing(int Channel, int Led0, GColour Col0, int Led1, GColour Col1)
	{
	    LedRingBits r[2];

        ZeroObj(r);

		EnableLedRing(false);

        #define SetLedBit(out, in, comp) \
	        if (Col##in.comp()) \
	        { \
	            int Bit0 = LedRingAddr[Led##in].comp; \
	            r[out][(Bit0>>3)] |= 1 << (Bit0&0x7); \
	        }

	    if (Led0 >= 0 && Led0 < 16)
	    {
    	    SetLedBit(1, 0, r);
    	    SetLedBit(1, 0, g);
    	    SetLedBit(1, 0, b);
	    }
	    
	    if (Led1 >= 0 && Led1 < 16)
	    {
    	    SetLedBit(0, 1, r);
    	    SetLedBit(0, 1, g);
    	    SetLedBit(0, 1, b);
	    }
	    
        if (WriteI2C(SC18IS602B_ADDR,
    		         Channel,
    		         (uint8_t*) &r[0], sizeof(r)))
		{
		    #if 0
		    printf("UpdateLedRing(%i, %i, %s, %i, %s) = { {%x,%x,%x,%x,%x,%x}, {%x,%x,%x,%x,%x,%x} }\n",
                Channel,
                Led0,
                Col0.GetStr(),
                Led1,
                Col1.GetStr(),
                r[0][0],r[0][1],r[0][2],r[0][3],r[0][4],r[0][5],
                r[1][0],r[1][1],r[1][2],r[1][3],r[1][4],r[1][5]);
            #endif

			// This forces the SPI transfer to complete before we do anything else
	        ClearIntLedRing();
        }

		EnableLedRing(true);
	}

	template<typename T>
	bool WriteI2C(int Addr, uint8_t Reg, T Value)
	{
		if (I2cHnd < 0)
		{
			printf("%s:%i - I2C device not open.\n", _FL);
			return false;
		}
		
		if (I2cAddr != Addr)
		{
			if (ioctl(I2cHnd, I2C_SLAVE, Addr) < 0)
			{
				printf("%s:%i - I2C device select failed: 0x%x\n", _FL, Addr);
				return false;
			}
			
			I2cAddr = Addr;
		}
		
		uint8_t Buf[16];
		Buf[0] = Reg;
		int Bytes = sizeof(Value);
		uint8_t *p = (uint8_t*)&Value;
		for (unsigned i=0; i<Bytes; i++)
		{
		    Buf[1+i] = p[Bytes-i-1];
		}		
		
		int Wr = write(I2cHnd, Buf, 1 + Bytes);
		if (Wr != (1 + Bytes))
		{
		    GString Val;
		    Val.Printf("[%i]{", Bytes);
    		for (unsigned i=0; i<Bytes; i++)
    		{
    		    GString s;
    		    s.Printf("%s%i", i?",":"", p[Bytes-i-1]);
    		    Val += s;
    		}
    		Val += "}";
		    
			printf("%s:%i - I2C write failed (Addr=0x%x, Reg=0x%x, Value=%s, Wr=%i)\n",
				_FL,
				Addr, Reg, Val.Get(), Wr);
			return false;
		}
		/*
		else if (Addr == 0x28)
		{
			printf("I2c.Write(%i)=%i {", Bytes, Wr);
			for (unsigned i=0; i<1+Bytes; i++)
			{
				printf("%02x,", Buf[i]);
			}
			printf("}\n");
		}
		*/

		return true;
	}

	bool WriteI2C(int Addr, uint8_t Reg, uint8_t *Ptr, int Bytes, bool Quiet = false)
	{
		if (I2cHnd < 0)
		{
			printf("%s:%i - I2C device not open.\n", _FL);
			return false;
		}
		
		if (I2cAddr != Addr)
		{
			if (ioctl(I2cHnd, I2C_SLAVE, Addr) < 0)
			{
				printf("%s:%i - I2C device select failed: 0x%x\n", _FL, Addr);
				return false;
			}
			
			I2cAddr = Addr;
		}
		
		uint8_t Buf[16];
		Buf[0] = Reg;
		if (Bytes > 15)
		{
			printf("%s:%i - Too many bytes.\n", _FL);
			return false;
		}
		
		for (unsigned i=0; i<Bytes; i++)
		{
		    Buf[1+i] = Ptr[i];
		}		
		
		int Wr = write(I2cHnd, Buf, 1 + Bytes);
		if (Wr != (1 + Bytes))
		{
			if (!Quiet)
			{
			    GString Val;
			    Val.Printf("[%i]{", Bytes);
	    		for (unsigned i=0; i<Bytes; i++)
	    		{
	    		    GString s;
	    		    s.Printf("%s%i", i?",":"", Ptr[i]);
	    		    Val += s;
	    		}
	    		Val += "}";
			    
				printf("%s:%i - I2C write failed (Addr=0x%x, Reg=0x%x, Value=%s, Wr=%i)\n",
					_FL,
					Addr, Reg, Val.Get(), Wr);
			}
			
			return false;
		}
		else if (!Quiet && Addr == 0x28)
		{
			printf("I2c.Write(%i)=%i {", Bytes, Wr);
			for (unsigned i=0; i<1+Bytes; i++)
			{
				printf("%02x,", Buf[i]);
			}
			printf("}\n");
		}
		
		return true;
	}
	
	int ReadI2C(int Addr, uint8_t Reg)
	{
		if (I2cHnd < 0)
		{
			printf("%s:%i - I2C device not open.\n", _FL);
			return -1;
		}

		if (I2cAddr != Addr)
		{
			if (ioctl(I2cHnd, I2C_SLAVE, Addr) < 0)
			{
				printf("%s:%i - I2C device select failed: 0x%x\n", _FL, Addr);
				return false;
			}
			
			I2cAddr = Addr;
		}
		
		int Wr = write(I2cHnd, &Reg, 1);
		if (Wr != 1)
		{
			printf("%s:%i - I2C write failed: %i (%i)\n", _FL, Wr, errno);
			return -1;
		}
		
		uint8_t Data = 0;
		if (read(I2cHnd, &Data, 1) != 1)
		{
			printf("%s:%i - I2C read failed.\n", _FL);
			return -1;
		}
		
		return Data;
	}
	
	void ADC_StartRead(uint8_t channel)
	{
		if (channel > 3)
			return;

		// Start with default values
		uint16 config =	ADS1115_REG_CONFIG_CQUE_NONE	| // Disable the comparator (default val)
						ADS1115_REG_CONFIG_CLAT_NONLAT	| // Non-latching (default val)
						ADS1115_REG_CONFIG_CPOL_ACTVLOW | // Alert/Rdy active low	(default val)
						ADS1115_REG_CONFIG_CMODE_TRAD	| // Traditional comparator (default val)
						ADS1115_REG_CONFIG_DR_1600SPS	| // 1600 samples per second (default)
						ADS1115_REG_CONFIG_MODE_SINGLE;	  // Single-shot mode (default)

		// Set PGA/voltage range
		config |= GAIN_TWOTHIRDS;

		// Set single-ended input channel
		switch (channel)
		{
			case 0:
				config |= ADS1115_REG_CONFIG_MUX_SINGLE_0;
				break;
			case 1:
				config |= ADS1115_REG_CONFIG_MUX_SINGLE_1;
				break;
			case 2:
				config |= ADS1115_REG_CONFIG_MUX_SINGLE_2;
				break;
			case 3:
				config |= ADS1115_REG_CONFIG_MUX_SINGLE_3;
				break;
		}

		// Set 'start single-conversion' bit
		config |= ADS1115_REG_CONFIG_OS_SINGLE;

		// Write config register to the ADC
		WriteI2C(ADS1115_ADDRESS, ADS1115_REG_POINTER_CONFIG, config);

	}

	int ADC_FinishRead()
	{
		if (I2cHnd < 0)
		{
			printf("%s:%i - I2C device not open.\n", _FL);
			return -1;
		}

		int Addr = ADS1115_ADDRESS;
		if (I2cAddr != Addr)
		{
			if (ioctl(I2cHnd, I2C_SLAVE, Addr) < 0)
			{
				printf("%s:%i - I2C device select failed: 0x%x\n", _FL, Addr);
				return -1;
			}

			I2cAddr = Addr;
		}

		uint8_t Buf = ADS1115_REG_POINTER_CONVERT;
		int Wr = write(I2cHnd, &Buf, 1);
		if (Wr != 1)
		{
			printf("%s:%i - I2C write failed: %i (%i)\n", _FL, Wr, errno);
			return -1;
		}

		uint8_t Data[2] = {0};
		if (read(I2cHnd, Data, 2) != 2)
		{
			printf("%s:%i - I2C read failed.\n", _FL);
			return -1;
		}

        int16_t Val = ((Data[0] << 8) | Data[1]);
		return MAX(0, Val);
	}
	
	bool SetupMCP23017(int Addr)
	{
		#define SetupPort(Port) \
			if (!WriteI2C(Addr, MCP23017_IODIR##Port, (uint8_t)0xff) || \
				!WriteI2C(Addr, MCP23017_GPPU##Port, (uint8_t)0xff)	|| \
				!WriteI2C(Addr, MCP23017_IPOL##Port, (uint8_t)0xff)) \
				return false;

		// Do each port
		SetupPort(A);
		SetupPort(B);

		return true;
	}
	
	uint32_t ReadMCP23017(int Addr)
	{
		int PortA = ReadI2C(Addr, MCP23017_GPIOA);
		if (PortA < 0)
			return -1;
		
		int PortB = ReadI2C(Addr, MCP23017_GPIOB);
		if (PortB < 0)
			return -1;
		
		return ((PortB & 0xff) << 8) | (PortA & 0xff);
	}
	
	GBgrx32 *Px()
	{
		return (GBgrx32*) Leds.channel[0].leds;
	}

	bool Update()
	{
		int r = ws2811_render(&Leds);
		return r == 0;
	}
	
	GMessage::Result OnEvent(GMessage *m)
	{
		switch (m->Msg())
		{
			case M_SET_BTN_MODE:
			{
				BtnModes[(int)m->A()] = (Hardware::ButtonModes)(int)m->B();
				break;
			}
			case M_SET_LED_COLOUR:
			{
				int Index = (int)m->A();
				GColour Col((uint32_t)m->B(), 32);
				
				GBgrx32 *p = Px();
				if (!p)
					break;
				if (Index < 7)
					p += Index;
				else
					p += 13 - (Index % 7);
				
				p->r = Col.r();
				p->g = Col.g();
				p->b = Col.b();
				
				Update();
				break;
			}
			case M_SEND_MIDI:
			{
				GAutoPtr<MidiBuf> a((MidiBuf*)m->A());
				if (MidiHnd < 0)
				{
					LgiTrace("%s:%i - Error: Midi handle not open.\n", _FL);
				}
				else if (a->Length() > 0)
				{					
					int Wr = write(MidiHnd, &a->First(), a->Length());
					if (Wr != a->Length())
						LgiTrace("%s:%i - Midi write error: %i bytes written of %i\n", _FL, Wr, a->Length());
				}
				else
				{
					LgiTrace("%s:%i - Error: Midi buffer empty.\n", _FL);
				}
				break;
			}
			case M_LED_RING_VALUE:
			{
			    int Index = (int)m->A();
			    if (Index >= 0 && Index < Encoders.Length())
			    {
    			    int Value = (int)m->B();
    			    Encoders[Index].SetValue(Value);

                    int Led = Encoders[Index].GetValue() >> 3;
                    // printf("M_LED_RING_VALUE Led=%i,%i\n", Led, Encoders[Index].Led);
                    if (Led != Encoders[Index].Led)
                    {
                        Encoders[Index].Led = Led;
                        Encoders[Index].Dirty = true;
				    }
			    }
			    else printf("%s:%i - Out of range index: %i\n", _FL, Index);
			    break;
			}
			case M_LED_RING_COLOUR:
			{
			    int Index = (int)m->A();
			    if (Index >= 0 && Index < Encoders.Length())
			    {
			        uint32_t c32 = (uint32_t) m->B();
			        if (Encoders[Index].Col.c32() != c32)
			        {
        			    Encoders[Index].Col.Set(c32, 32);
        			    Encoders[Index].Dirty = true;
    			    }
    			}
			    else printf("%s:%i - Out of range index: %i\n", _FL, Index);
			    break;
			}
			case M_SET_PARAM:
			{
			    // printf("%s:%i - Got M_SET_PARAM\n", _FL);
				Callback->OnParam((int)m->A(), (int)m->B());
			    break;
			}
			default:
			{
			    return Callback->OnEvent(m);
			    break;
			}
		}
		
		return 0;
	}
	
	int Main()
	{
		uint32_t CurBtns = 0;
		uint32_t HeldBtns = 0;

		int AdcPlugged = -1;
		uint64_t AdcPlugTs = 0;
		int AdcValue = 0;
		int AdcScaled = 0;
		int AdcMin = 17000;
		int AdcMax = 0;

		uint32_t AllHwButtonMasks = 0;
		struct timeval t = {0 /* seconds */, 100 /* micro seconds or .1ms */};

        // This is true until we start receiving messages. This indicates that the Mc2Logic
        // object has got it's interface pointers and is ready to run. If we start processing
        // MIDI and buttons before that then things don't work correctly.
		bool InInitialisationPhase = true;

		printf("Hardware polling thread running...\n");
	
	    // Make a mask of all the hardware buttons
	    for (int i=0; i<CountOf(HwButtonMasks); i++)
	    {
	        AllHwButtonMasks |= HwButtonMasks[i];
	    }

		// Start first ADC conversion
		uint64_t LastAdcRead = LgiCurrentTime();
		ADC_StartRead(0);

		while (MidiHnd >= 0)
		{
			fd_set r;
			FD_ZERO(&r);
			FD_SET(MidiHnd, &r);
			
			// Check message que
			if (MsgQue.Length()) // This is safe to do without locking			
			{
			    if (Lck.Lock(_FL))
			    {
			        // Only process one message at a time... to keep this loop
			        // running as much as possible
			        if (MsgQue.Length() > 0)
			        {
			            GMessage *m = MsgQue[0];
			            OnEvent(m);
			            DeleteObj(m);
			            MsgQue.DeleteAt(0, true);
			            
			            if (InInitialisationPhase)
			            {
			                // First message received (from Mc2Logic::SetInterfaces)
			                // So exit the initialisation phase and start processing
			                // incoming MIDI.
			                InInitialisationPhase = false;
			            }
			        }
			        Lck.Unlock();
			    }
			}
			
			if (InInitialisationPhase)
			    continue;

			if (TimerTs)
			{
			    uint64 Now = LgiCurrentTime();
			    if (Now >= TimerTs)
			    {
			        Callback->OnTimer();
			        TimerTs = 0;
			    }
			}
			
			int v = select(MidiHnd+1, &r, 0, 0, &t);
			if (v > 0 && FD_ISSET(MidiHnd, &r))
			{
			    uint64_t Start = LgiCurrentTime();
			    
				uint8_t Buf[256];
				int Rd = read(MidiHnd, Buf, sizeof(Buf));
				if (Rd > 0)
				{
					MidiTs = LgiCurrentTime();
					if (!MidiConnected)
					{
						MidiConnected = true;
						Callback->OnConnection(true);
					}
					
					MidiIn.Add(Buf, Rd);

					int Pkt;
					while (Pkt = MidiPacketSize(&MidiIn.First(), MidiIn.Length()))
					{
					    uint8_t *Ptr = &MidiIn.First();
						Callback->OnMidiIn(Ptr, Pkt);
						
						int Remaining = MidiIn.Length() - Pkt;
						memmove(Ptr, Ptr + Pkt, Remaining);
						MidiIn.Length(Remaining);
						if (MidiIn.Length() == 0)
						    break;
					}
				}

			    uint64_t End = LgiCurrentTime();
			    if (End - Start > 5)
			        printf("%s:%i - Midi input processing took %i ms\n", _FL, (int) (End - Start));
			}
			else if (v < 0)
			{
				printf("%s:%i - select failed with %i\n", _FL, v);
			}
			else if (I2cHnd >= 0)
			{
				// Read some buttons
				uint32_t Btns =	 ReadMCP23017(MCP23017_ADDR1) |
								(ReadMCP23017(MCP23017_ADDR2) << 16);
				int BitsOn = 0;
				int NonEncoderBits = AllHwButtonMasks & Btns;
				for (int i=0; i<sizeof(Btns)<<3; i++)
				{
					if ((1 << i) & NonEncoderBits)
						BitsOn++;
				}

   				uint64 Now = LgiCurrentTime();
				if (BitsOn > 4)
				{
					printf("%s:%i - Btn read failed: 0x%x (BitsOn=%i)\n", _FL, Btns, BitsOn);
				}
				else
				{
					if (Btns != CurBtns)
					{
						#if DEBUG_SHOW_BTN_BITS
						printf("0x%08.8x\n", Btns);
						#endif
						
						// Check encoder positions
						for (int i=0; i<CountOf(EncoderShifts); i++)
						{
						    int OldValue = Encoders[i].GetValue();
							int Shift = EncoderShifts[i];
							int OldBits = (CurBtns >> Shift) & 3;
							int NewBits = (Btns >> Shift) & 3;
							bool Up = Encoders[i].Update(NewBits, Now, DebugEnc == i);

							if (OldBits != NewBits)
							{
							    /*
							    if (DebugEnc == i)
							    {
    							    printf("Enc[%i], %i->%i, Up=%i\n",
    							        i, OldBits, NewBits, Up);
    							}
    							*/
							    
							    CurBtns = (CurBtns & ~(3 << Shift)) | (NewBits << Shift);
							}

							if (Up)
							{
            					#if DEBUG_HARDWARE_EVENTS
            					printf("Encoder[%i]=%i\n", i, Encoders[i].Value);
            					#endif
            					int Val = Encoders[i].GetValue();
            					if (Val >= 0 && Val < 128)
            					{
            					    // printf("%s:%i - Calling OnParam(%i)\n", _FL, i);
							        Callback->OnParam(i, Encoders[i].GetValue());
							    }
							}
						}
						
						// Check hardware buttons
						for (int i=0; i<CountOf(HwButtonMasks); i++)
						{
							int Mask = HwButtonMasks[i];
							bool CurrrentlyDown = (Mask & Btns) != 0;
							bool PreviouslyDown = (Mask & CurBtns) != 0;
							
							if (CurrrentlyDown ^ PreviouslyDown)
							{
								bool Notify = false;
								uint64 Diff = Now - BtnTs[i];

								if (Diff > MIN_BUTTON_EVENT_MS)
								{
									Notify = true;
	   								BtnTs[i] = Now;

									if (CurrrentlyDown)
									{
										// printf("Down[%i] "LGI_PrintfInt64"\n", i, Diff);
										CurBtns |= Mask;
									}
									else
									{
										// printf("Up[%i] "LGI_PrintfInt64"\n", i, Diff);
										CurBtns &= ~Mask;
										HeldBtns &= ~Mask;
									}
								}
								
								if (Notify)
								{
                    				#if DEBUG_HARDWARE_EVENTS
                    				printf("Btn[%i]=%i Mask=%x\n", i, CurrrentlyDown, Mask);
                    				#endif
                    				Callback->OnButton(i, CurrrentlyDown, false);
									
									if (CurrrentlyDown)
									{
									    switch (i)
									    {
									        case BtnEncoder2:
									            DebugEnc = 1;
									            break;
									        case BtnEncoder3:
									            DebugEnc = 2;
									            break;
									        case BtnEncoder4:
									            DebugEnc = 3;
									            break;
									        case BtnEncoder5:
									            DebugEnc = 4;
									            break;
									        case BtnEncoder6:
									            DebugEnc = 5;
									            break;
									    }
									}
								}
							}
						}
					}

					// Check buttons for held status
					for (int i=0; i<CountOf(HwButtonMasks); i++)
					{
						int Mask = HwButtonMasks[i];
						if ((CurBtns & Mask) != 0 &&
							(HeldBtns & Mask) == 0)
						{
							uint64 Diff = Now - BtnTs[i];
							if (Diff >= HOLD_BUTTON_EVENT_MS)
							{
								HeldBtns |= Mask;

                				#if DEBUG_HARDWARE_EVENTS
                				printf("Held[%i]\n", i);
                				#endif
                				Callback->OnButton(i, true, true);
								// View->PostEvent(M_BUTTON_HELD, i);
							}
						}
					}
				}
				
				// Do A2D read
				if (Now - LastAdcRead > 7)
				{
					int Val = ADC_FinishRead();
					if (Val < 0 || Val > 60000)
					{
				        printf("%s:%i - Invalid read: %i (time=%ims)\n", _FL, Val, (int)(Now - LastAdcRead));
					}
					else if (abs(Val - AdcValue) > ADC_THRESHOLD)
					{
						AdcValue = Val;
						
						int Scaled = 127;
						int Plugged = AdcValue < ADC_UNPLUGGED;
						if (AdcPlugged < 0 || Plugged ^ AdcPlugged)
						{
							AdcPlugTs = Now;
							if (AdcPlugged = Plugged)
							{
								// Unplugged -> Plugged
								printf("%s:%i - Expression pedal plugged in: %i\n", _FL, AdcValue);
							}
							else
							{
								// Plugged -> Unplugged
								printf("%s:%i - Expression pedal unplugged: %i\n", _FL, AdcValue);
							}
						}
						
						if (!AdcPlugged)
						{
							AdcMin = 17000;
							AdcMax = 0;
						}
						else if (Now - AdcPlugTs > ADC_REPLUG_MS)
						{
							if (AdcValue < AdcMin)
								AdcMin = AdcValue;
							if (AdcValue > AdcMax)
								AdcMax = AdcValue;
							Scaled = (double)(AdcValue - AdcMin) * 127.5 / (AdcMax - AdcMin + 1);
						}
						
						if (Scaled != AdcScaled)
						{					
							AdcScaled = Scaled;
							#if DEBUG_ANALOG_INPUTS
							printf("ADC[0]=%i (min=%i,raw=%i,max=%i,ts="LGI_PrintfInt64")\n", AdcScaled, AdcMin, AdcValue, AdcMax, Now - AdcPlugTs);
							#endif
							
            				#if DEBUG_HARDWARE_EVENTS
            				printf("Pedal=%i\n", AdcScaled);
            				#endif
            				Callback->OnParam(7, AdcScaled);
						}
					}

					// Start the next read
					ADC_StartRead(0);
					LastAdcRead = LgiCurrentTime();
				}
				
				if (MidiConnected &&
					Now - MidiTs > MIDI_TIMEOUT)
				{
					MidiConnected = false;					
					Callback->OnConnection(false);
				}
				
				if (Now - LedRingUpdateTs >= 10)
				{
				    LedRingUpdateTs = Now;
				    
        			// Check LED rings for dirty flag
        			bool IsDirty = false;
        			for (unsigned i = 0; i < Encoders.Length(); i++)
        			{
        			    if (Encoders[i].Dirty)
        			    {
        			    	IsDirty = true;
        			    	break;
    				    }
        			}
        			if (IsDirty)
        			{
		    			// EnableLedRing(false);
	        			for (unsigned Grp = 0; Grp < Encoders.Length(); Grp+=2)
	        			{
	        			    if (Encoders[Grp].Dirty ||
	        			        Encoders[Grp+1].Dirty)
	        			    {
	        			    	int Index = 2 - (Grp >> 1);
        				        if (UpdateLedRing(  1 << Index,
	        				                        Encoders[Grp].Led,
	        				                        Encoders[Grp].Col,
	        				                        Encoders[Grp+1].Led,
	        				                        Encoders[Grp+1].Col))
	        				    {	        				    
	        				    	Encoders[Grp].Dirty = false;
	        			        	Encoders[Grp+1].Dirty = false;
	        			        }
	        			        break;
	    				    }
	        			}
		    			// EnableLedRing(true);
	        		}
				}
			}
		}
	
		return 0;
	}
};

Hardware::Hardware(HardwareEvents *Callback)
{
	d = new HardwarePriv(this, Callback);
}

Hardware::~Hardware()
{
    printf("~Hardware() start\n");
	delete d;
    printf("~Hardware() end\n");
}

void Hardware::SetInterface(GView *v)
{
}

int Hardware::GetButtons()
{
	return HW_BUTTONS + ENC_BUTTONS;
}

bool Hardware::SetButtonMode(int Index, ButtonModes Mode)
{
	if (Index >= GetButtons())
		return false;
	
	return d->PostEvent(M_SET_BTN_MODE, (GMessage::Param)Index, (GMessage::Param)Mode);
}

int Hardware::GetParams()
{
	return ENCODERS + ANALOG_INPUTS;
}

bool Hardware::SetParamRange(int Index, int Min, int Max)
{
	return false;
}

bool Hardware::SetParamValue(int Index, int Value)
{
	return false;
}

bool Hardware::SetParamColour(int Index, GColour Colour, ParamColourMode Mode)
{
	return false;
}

int Hardware::GetLeds()
{
	return LED_COUNT;
}

bool Hardware::SetLed(int Index, GColour Col)
{
	if (Index < 0 || Index >= GetLeds())
	{
		LgiTrace("%s:%i - Invalid LED index '%i'\n", _FL, Index);
		return false;
	}
	
	d->PostEvent(M_SET_LED_COLOUR, (GMessage::Param)Index, (GMessage::Param)Col.c32());
}

bool Hardware::SendMidi(uint8_t *Buf, size_t Length)
{
	if (!Buf || Length < 1)
	{
		printf("%s:%i - Error: SendMidi invalid parameter.\n", _FL);
		return false;
	}

	#if 0
	printf("MidiOut={");
	for (int k=0; k<Length; k++)
	{
		printf("%02.2x%s", Buf[k], k < Length - 1 ? ", " : "}\n");
	}
	#endif

	MidiBuf *a = new MidiBuf;
	if (!a)
	{
		printf("%s:%i - Error: SendMidi invalid parameter.\n", _FL);
		return false;
	}

    a->Add(Buf, Length);
	return d->PostEvent(M_SEND_MIDI, a);
}

bool Hardware::PostEvent(int Cmd, GMessage::Param a, GMessage::Param b)
{
	return d->PostEvent(Cmd, a, b);
}

bool Hardware::SetTimer(int Ms)
{
	d->TimerTs = LgiCurrentTime() + Ms;
	return true;
}

GViewI *Hardware::GetInterface()
{
	return NULL;
}
