/*	PIC32MZ Demo Code
**
**	Copyright (C) 2018-2021 Herbert Poetzl
**
**	This program is free software; you can redistribute it and/or modify
**    	it under the terms of the GNU General Public License 2 as published 
**	by the Free Software Foundation.
*/

#include <xc.h>
#include <stdint.h>
#include <stdbool.h>

	// DEVCFG0
#pragma config BOOTISA = MIPS32
#pragma config ICESEL = ICS_PGx1
#pragma config FECCCON = OFF_UNLOCKED
#pragma config FSLEEP = 0

	// DEVCFG1
#pragma	config FDMTEN = OFF
#pragma	config FWDTEN = OFF
#pragma config POSCMOD = OFF
#pragma config OSCIOFNC = ON
#pragma config FSOSCEN = OFF
#pragma config FNOSC = SPLL
#pragma config FCKSM = CSECMD

	// DEVCFG2
#pragma config FPLLICLK = PLL_FRC
#pragma config FPLLIDIV = DIV_2
#pragma config FPLLRNG = RANGE_5_10_MHZ
#pragma config FPLLMULT = MUL_100
#pragma config FPLLODIV = DIV_4
// #pragma config UPLLEN = OFF
#pragma config UPLLFSEL = FREQ_24MHZ

	// DEVCFG3
#pragma config USERID = 0xC0DE
#pragma config FMIIEN = OFF
#pragma config PGL1WAY = OFF
#pragma config PMDL1WAY = OFF
#pragma config IOL1WAY = OFF
#pragma config FUSBIDIO = OFF

	// DEVCP0
#pragma config CP = OFF


#define	MCU_16BIT


#define VERS_0_14

	/* RPC14/SOSCO */
#define ICSP_E_MCLR_O	LATCbits.LATC14
#define ICSP_E_MCLR_I	PORTCbits.RC14
#define ICSP_E_MCLR_T	TRISCbits.TRISC14

#ifdef	VERS_0_14

	/* RPF5/SCL5 */
#define	ICSP_E_PCLK_O	LATFbits.LATF5
#define	ICSP_E_PCLK_I	PORTFbits.RF5
#define	ICSP_E_PCLK_T	TRISFbits.TRISF5
#define	ICSP_E_PCLK_U	CNPUFbits.CNPUF5
#define	ICSP_E_PCLK_D	ODCFbits.ODCF5

	/* RPF4/SDA5 */
#define	ICSP_E_PDAT_O	LATFbits.LATF4
#define ICSP_E_PDAT_I	PORTFbits.RF4
#define ICSP_E_PDAT_T	TRISFbits.TRISF4
#define ICSP_E_PDAT_U	CNPUFbits.CNPUF4
#define	ICSP_E_PDAT_D	ODCFbits.ODCF4

#define	I2C_E		5
#define I2C_E_BB	_BB
#else
	/* RA2/SCL2 */
#define	ICSP_E_PCLK_O	LATAbits.LATA2
#define	ICSP_E_PCLK_I	PORTAbits.RA2
#define	ICSP_E_PCLK_T	TRISAbits.TRISA2
#define	ICSP_E_PCLK_U	CNPUAbits.CNPUA2
#define	ICSP_E_PCLK_D	ODCFbits.ODCA2

	/* RA3/SDA2 */
#define	ICSP_E_PDAT_O	LATAbits.LATA3
#define ICSP_E_PDAT_I	PORTAbits.RA3
#define ICSP_E_PDAT_T	TRISAbits.TRISA3
#define ICSP_E_PDAT_U	CNPUAbits.CNPUA3
#define	ICSP_E_PDAT_D	ODCFbits.ODCA3

#define	I2C_E		2
#define I2C_E_BB	_BB
#endif

	/* RPC14/SOSCI */
#define ICSP_W_MCLR_O	LATCbits.LATC13
#define ICSP_W_MCLR_I	PORTCbits.RC13
#define ICSP_W_MCLR_T	TRISCbits.TRISC13

	/* RPF8/SCL3 */
#define	ICSP_W_PCLK_O	LATFbits.LATF8
#define	ICSP_W_PCLK_I	PORTFbits.RF8
#define	ICSP_W_PCLK_T	TRISFbits.TRISF8
#define ICSP_W_PCLK_U	CNPUFbits.CNPUF8
#define ICSP_W_PCLK_D	ODCFbits.ODCF8

	/* RPF2/SDA3 */
#define	ICSP_W_PDAT_O	LATFbits.LATF2
#define ICSP_W_PDAT_I	PORTFbits.RF2
#define ICSP_W_PDAT_T	TRISFbits.TRISF2
#define ICSP_W_PDAT_U	CNPUFbits.CNPUF2
#define ICSP_W_PDAT_D	ODCFbits.ODCF2

#define	I2C_W		3
#define	I2C_W_BB	_BB



static inline
void	unlock(void)
{
	SYSKEY = 0xAA996655;
	SYSKEY = 0x556699AA;
}

static inline
void	lock(void)
{
	SYSKEY = 0x33333333;
}

static inline
void	irq_disable(void)
{
	asm volatile("di");
	asm volatile("ehb");
}

static inline
void	irq_enable(void)
{
	asm volatile("ei");
}


void	init_pbus(void)
{
	unlock();
	PB2DIVbits.PBDIV = 0b000001;	// divide by 2
	PB2DIVbits.ON = 1;

	PB7DIVbits.PBDIV = 0b000000;	// divide by 1
	PB7DIVbits.ON = 1;
	lock();
}



#define	DEF_X(L, l, n, N, B)				\
	N ## B ## _N(L, l, ICSP_ ## L, I2C ## n)
#define DEF_Y(L, l, n, N, B)	DEF_X(L, l, n, N, B)

#define	DEF(L, l, N)					\
	DEF_Y(L, l, I2C_ ## L, N, )
#define	DEF_BB(L, l, N)					\
	DEF_Y(L, l, I2C_ ## L, N, I2C_ ## L ## _BB)

#define	ICSP_INIT_N(L, l, P, I)				\
void	icsp_init_ ## l(void)				\
{							\
	P ## _MCLR_T = 0;	/* MCLR out */		\
	P ## _PCLK_T = 0;	/* PCLK out */		\
	P ## _PDAT_T = 0;	/* PDAT out */		\
							\
	I ## CONbits.ON = 0;	/* I2C off */		\
}

#define ICSP_INIT(L, l)	DEF(L, l, ICSP_INIT)

ICSP_INIT(E, e)
ICSP_INIT(W, w)


#define	I2C_INIT_N(L, l, P, I)				\
void	i2c_init_ ## l(void)				\
{							\
	P ## _PCLK_U = 1;				\
	P ## _PDAT_U = 1;				\
	P ## _PCLK_D = 1;				\
	P ## _PDAT_D = 1;				\
							\
	I ## ADD = 0xFF;				\
	I ## MSK = 0xFF;				\
							\
	I ## BRG = 128;					\
							\
	P ## _PDAT_O = 0;	/* SDA low */		\
	P ## _PDAT_T = 0;	/* SDA out */		\
	P ## _PCLK_O = 1;	/* SCL high */		\
	P ## _PCLK_T = 1;	/* SCL in */		\
							\
	I ##CONbits.ON = 1;				\
}

#define	I2C_INIT_BB_N(L, l, P, I)			\
void	i2c_init_ ## l(void)				\
{							\
	P ## _PCLK_U = 1;				\
	P ## _PDAT_U = 1;				\
	P ## _PCLK_D = 1;				\
	P ## _PDAT_D = 1;				\
							\
	P ## _PDAT_O = 1;	/* SDA high */		\
	P ## _PDAT_T = 0;	/* SDA out */		\
	P ## _PCLK_O = 1;	/* SCL high */		\
	P ## _PCLK_T = 0;	/* SCL out */		\
}

#define I2C_INIT(L, l) DEF_BB(L, l, I2C_INIT)

I2C_INIT(E, e)
I2C_INIT(W, w)


void	init_uart2(void)
{
	irq_disable();

	U2MODEbits.ON = 0;

	TRISEbits.TRISE8 = 0;		// U2TX out
	TRISEbits.TRISE9 = 1;		// U2RX in
	ANSELEbits.ANSE8 = 0;		// digital
	ANSELEbits.ANSE9 = 0;		// digital

	CFGCONbits.IOLOCK = 0;
	RPE8Rbits.RPE8R = 0b0010;	// U2TX
	U2RXRbits.U2RXR = 0b1101;	// RPE9
	CFGCONbits.IOLOCK = 1;

	INTCONbits.MVEC = 1;		// Multi Vector Interrupts
	PRISSbits.SS0 = 0;		// Normal Register Set
	PRISSbits.PRI7SS = 7;		// Assign Shadow Register Set

	IPC36bits.U2TXIP = 7;		// Interrupt priority of 7
	IPC36bits.U2TXIS = 0;		// Interrupt sub-priority of 0
	IPC36bits.U2RXIP = 7;		// Interrupt priority of 7
	IPC36bits.U2RXIS = 0;		// Interrupt sub-priority of 0

	IEC4SET = _IEC4_U2RXIE_MASK;	// Rx INT Enable
	IFS4bits.U2TXIF = 0;		// Clear Tx flag
	IFS4bits.U2RXIF = 0;		// Clear Rx flag

	U2BRG = 24;			// 1MBaud @ 50MHz
	U2STA = 0;
	
	U2MODEbits.BRGH = 1;
	U2MODEbits.PDSEL = 0b00;
	U2MODEbits.STSEL = 0;
	U2MODEbits.UEN = 0b00;
	U2MODEbits.ON = 1;
	U2STASET = 0x9400;		// Enable Transmit and Receive

	irq_enable();
}


static inline
void	i2c_bb_delay(unsigned cnt)
{
	unsigned i;
	while (cnt--)
	    for (i=0; i<250; i++)
		asm("nop");
}

#define	I2C_START_N(L, l, P, I)			\
uint8_t	i2c_ ## l ## _start(void)		\
{						\
	I ## CONbits.SEN = 1;			\
	while (I ## CONbits.SEN);		\
	return I ## STAT & 0xFF;		\
}						\

#define	I2C_START_BB_N(L, l, P, I)		\
uint8_t	i2c_ ## l ## _start(void)		\
{						\
	P ## _PDAT_O = 0;	/* SDA low */	\
	i2c_bb_delay(2);			\
	P ## _PCLK_O = 0;	/* SCL low */	\
	i2c_bb_delay(2);			\
	return 0xFF;				\
}

#define I2C_START(L, l) DEF_BB(L, l, I2C_START)

static inline I2C_START(W, w)
static inline I2C_START(E, e)


#define	I2C_RESTART_N(L, l, P, I)		\
uint8_t	i2c_ ## l ## _restart(void)		\
{						\
	I ## CONbits.RSEN = 1;			\
	while (I ## CONbits.RSEN);		\
	return I ## STAT & 0xFF;		\
}

#define	I2C_RESTART_BB_N(L, l, P, I)		\
uint8_t	i2c_ ## l ## _restart(void)		\
{						\
	P ## _PDAT_O = 1;	/* SDA high */	\
	i2c_bb_delay(2);			\
	P ## _PCLK_O = 1;	/* SCL high */	\
	i2c_bb_delay(2);			\
	P ## _PDAT_O = 0;	/* SDA low */	\
	i2c_bb_delay(2);			\
	P ## _PCLK_O = 0;	/* SCL low */	\
	i2c_bb_delay(2);			\
	return 0xFF;				\
}

#define I2C_RESTART(L, l) DEF_BB(L, l, I2C_RESTART)

static inline I2C_RESTART(W, w)
static inline I2C_RESTART(E, e)


#define	I2C_STOP_N(L, l, P, I)			\
uint8_t	i2c_ ## l ## _stop(void)		\
{						\
	if ((I ## CON & 0x1F) == 0)		\
	    I ## CONbits.PEN = 1;		\
	return I ## CON & 0x1F;			\
}

#define	I2C_STOP_BB_N(L, l, P, I)		\
uint8_t	i2c_ ## l ## _stop(void)		\
{						\
	P ## _PCLK_O = 1;	/* SCL high */	\
	i2c_bb_delay(2);			\
	P ## _PDAT_O = 1;	/* SDA high */	\
	i2c_bb_delay(2);			\
	return 0xFF;				\
}

#define I2C_STOP(L, l) DEF_BB(L, l, I2C_STOP)

static inline I2C_STOP(W, w)
static inline I2C_STOP(E, e)


#define	I2C_WRITE_N(L, l, P, I)			\
bool	i2c_ ## l ## _write(uint8_t byte)	\
{						\
	I ## TRN = byte;			\
	while (I ## STATbits.TRSTAT);		\
	return I ## STATbits.ACKSTAT;		\
}

#define	I2C_WRITE_BB_N(L, l, P, I)		\
bool	i2c_ ## l ## _write(uint8_t byte)	\
{						\
	bool r = 0;				\
						\
	for (int i=0; i<9; i++) {		\
	    if (i == 8)	{	/* ACK */	\
	    	P ## _PDAT_T = 1;		\
	    	P ## _PDAT_O = 1;		\
	    } else {				\
		bool bit = (byte & 0x80);	\
	    	P ## _PDAT_O = bit;		\
	    }					\
	    i2c_bb_delay(1);			\
	    P ## _PCLK_O = 1;	/* SCL high */	\
	    i2c_bb_delay(1);			\
	    if (i == 8)	{	/* ACK */	\
		r = P ## _PDAT_I;		\
	    } else {				\
		byte <<= 1;			\
	    }					\
	    i2c_bb_delay(2);			\
	    P ## _PCLK_O = 0;	/* SCL low */	\
	}					\
	P ## _PDAT_T = 0;	/* Cleanup */	\
	return r;				\
}

#define I2C_WRITE(L, l) DEF_BB(L, l, I2C_WRITE)

static inline I2C_WRITE(W, w)
static inline I2C_WRITE(E, e)


#define	I2C_READ_N(L, l, P, I)			\
uint8_t	i2c_ ## l ## _read(bool ackdt)		\
{						\
	while (I ## STATbits.RBF)		\
	    (void)I ## RCV;			\
	if (I ## CON & 0x1F)			\
	    return 0xFF;			\
						\
	I ## CONbits.RCEN = 1;			\
	while (!I ## STATbits.RBF);		\
						\
	I ## CONbits.ACKDT = ackdt;		\
	I ## CONbits.ACKEN = 1;			\
	while (I ## CONbits.ACKEN);		\
						\
	return I ## RCV;			\
}

#define	I2C_READ_BB_N(L, l, P, I)		\
uint8_t	i2c_ ## l ## _read(bool ackdt)		\
{						\
	uint8_t byte = 0;			\
						\
	P ## _PDAT_T = 1;	/* Prep */	\
	for (int i=0; i<9; i++) {		\
	    if (i == 8)	{	/* ACK */	\
	    	P ## _PDAT_O = ackdt;		\
		P ## _PDAT_T = 0;		\
	    } else {				\
		byte <<= 1;			\
	    }					\
	    i2c_bb_delay(1);			\
	    P ## _PCLK_O = 1;	/* SCL high */	\
	    i2c_bb_delay(1);			\
	    if (i < 8)	{			\
	    	byte |= (P ## _PDAT_I) ? 1 : 0;	\
	    }					\
	    i2c_bb_delay(2);			\
	    P ## _PCLK_O = 0;	/* SCL low */	\
	}					\
	return byte;				\
}

#define I2C_READ(L, l) DEF_BB(L, l, I2C_READ)

static inline I2C_READ(W, w)
static inline I2C_READ(E, e)


static inline
void	set_mclr_w(unsigned val)
{
	ICSP_W_MCLR_O = (val & 1) ? 1 : 0;
}


static inline
void	set_mclr_e(unsigned val)
{
	ICSP_E_MCLR_O = (val & 1) ? 1 : 0;
}



static inline
void	uart2_ch(char ch)
{
	while (U2STAbits.UTXBF);
	U2TXREG = ch;
}

static inline
void	uart2_hex(uint8_t hex)
{
	hex &= 0xF;
	if (hex > 9)
	    uart2_ch(hex + 'A' - 10);
	else
	    uart2_ch(hex + '0');
}

static inline
void	uart2_byte(uint8_t val)
{
	uart2_hex(val >> 4);
	uart2_hex(val);
}

static inline
void	uart2_word(uint16_t val)
{
	uart2_byte(val >> 8);
	uart2_byte(val);
}

static inline
void	uart2_long(uint32_t val)
{
	uart2_word(val >> 16);
	uart2_word(val);
}

static inline
void	uart2_str0(const char *str)
{
	while (*str)
	    uart2_ch(*str++);
}


static inline
void	delay(unsigned cnt)
{
	unsigned i;
	while (cnt--)
	    for (i=0; i<200; i++);
}




void __attribute__((vector(_UART2_RX_VECTOR), interrupt(IPL7SRS))) uart2_isr(void)
{
	while (U2STAbits.URXDA) {	// process buffer 
	    char ch = U2RXREG;

	    uart2_ch(ch);	// echo back
	}
       
	IFS4CLR = _IFS4_U2RXIF_MASK;	// clear UART2 Rx IRQ
}


#define	I2C_GET(p)				\
uint8_t	p ## _get(uint8_t idx)			\
{						\
	uint8_t val;				\
						\
	p ## _start();				\
	p ## _write(0x20);			\
	p ## _write(idx);			\
	p ## _restart();			\
	p ## _write(0x21);			\
	val = p ## _read(1);			\
	p ##_stop();				\
						\
	return val;				\
}

I2C_GET(i2c_w)
I2C_GET(i2c_e)

#define	I2C_SET(p)				\
void	p ## _set(uint8_t idx, uint8_t val)	\
{						\
	p ## _start();				\
	p ## _write(0x20);			\
	p ## _write(idx);			\
	p ## _write(val);			\
	p ## _stop();				\
}

I2C_SET(i2c_w)
I2C_SET(i2c_e)

#define	I2C_GETN(p)				\
void	p ## _getn(uint8_t idx, 		\
		   uint8_t *data,		\
		   uint8_t n)			\
{						\
	p ## _start();				\
	p ## _write(0x20);			\
	p ## _write(idx);			\
	p ## _restart();			\
	p ## _write(0x21);			\
	while (n--)				\
	    *data++ = p ## _read(n == 0);	\
	p ## _stop();				\
}

I2C_GETN(i2c_w)
I2C_GETN(i2c_e)

#define	I2C_SETN(p)				\
void	p ## _setn(uint8_t idx,			\
		   uint8_t *data,		\
		   uint8_t n)			\
{						\
	p ## _start();				\
	p ## _write(0x20);			\
	p ## _write(idx);			\
	while (n--)				\
	    p ## _write(*data++);		\
	p ## _stop();				\
}

I2C_SETN(i2c_w)
I2C_SETN(i2c_e)


uint8_t bitidx(uint8_t val)
{
	uint8_t i;

	for (i=0; i<8; i++)
	    if (val & 1)
		return i;
	    else
		val >>= 1;
	return 0xF;
}

static inline
void	delay_us(unsigned cnt)
{
	while (cnt--) {
	    for (unsigned i=0; i<49; i++)
		asm("nop");
	}
}

#define	delay_ms(t) delay_us(1000*(t))



int	main(void)
{
	static uint8_t rgb[4];
	static uint8_t rgbt[4];
	static uint8_t data[16];
	static uint8_t qe[2];
	static uint32_t timeout = 0;
	static bool flush = false;


	init_pbus();
	init_uart2();

	icsp_init_w();
	icsp_init_e();

	i2c_init_w();
	i2c_init_e();

	delay_ms(1);
	uart2_str0("\f\n\rAXIOM Remote Demo v0.2\n\r");

	delay_ms(1);
	set_mclr_w(1);
	set_mclr_e(1);

	delay_ms(1);

	rgb[0] = 0x18;
	rgb[1] = 0x28;
	rgb[2] = 0x38;
	rgb[3] = 0x01;
	i2c_w_setn(0x20, rgb, 4);
	uart2_str0("rgb set ...\n\r");

	qe[0] = 0x0;
	qe[1] = 0x0;


	while (1) {
	    // asm volatile("wait");
	    flush = false;

	    i2c_e_getn(0x00, data, 3);

	    if (data[0] || data[1] || data[2]) {
		uart2_ch('e');
		uart2_byte(data[0]);
		uart2_byte(data[1]);
		uart2_byte(data[2]);
		uart2_ch('.');

		rgbt[0] = bitidx(data[0]);
		rgbt[1] = bitidx(data[1]);
		rgbt[2] = bitidx(data[2]);

		rgbt[3] = 1;

		i2c_w_setn(0x20, rgbt, 4);

		i2c_e_getn(0x04, data, 3);
		uart2_byte(data[0]);
		uart2_byte(data[1]);
		uart2_byte(data[2]);
		uart2_ch(',');

		timeout = 1000;	
		flush = true;
	    }

	    i2c_w_getn(0x00, data, 3);

	    if ((data[0] & 0x3F) || data[1] || (data[2] & 0x1F)) {
		uart2_ch('w');
		uart2_byte(data[0]);
		uart2_byte(data[1]);
		uart2_byte(data[2]);
		uart2_ch('.');

		rgbt[0] = bitidx(data[0]);
		rgbt[1] = bitidx(data[1]);
		rgbt[2] = bitidx(data[2]);

		rgbt[3] = 1;

		i2c_w_setn(0x20, rgbt, 4);

		i2c_w_getn(0x04, data, 3);
		uart2_byte(data[0]);
		uart2_byte(data[1]);
		uart2_byte(data[2]);
		uart2_ch(',');

		timeout = 1000;	
		flush = true;
	    }

	    i2c_w_getn(0x10, data, 2);

	    if (data[0] != qe[0]) {
		qe[0] = data[0];
		uart2_ch('a');
		uart2_byte(qe[0]);

		rgbt[0] = qe[0] & 0xF;
		rgbt[1] = qe[0] & 0xF;
		rgbt[2] = qe[0] & 0xF;
		rgbt[3] = 1;

		i2c_w_setn(0x20, rgbt, 4);

		uart2_ch(',');

		timeout = 3000;	
		flush = true;
	    }
	    
	    if (data[1] != qe[1]) {
		qe[1] = data[1];
		uart2_ch('b');
		uart2_byte(qe[1]);

		rgbt[0] = qe[1] & 0xF;
		rgbt[1] = qe[1] & 0xF;
		rgbt[2] = qe[1] & 0xF;
		rgbt[3] = 1;

		i2c_w_setn(0x20, rgbt, 4);

		uart2_ch(',');

		timeout = 3000;	
		flush = true;
	    }
	    
	    if (flush)
		uart2_ch('\n');

	    // data[0] = i2c3_get(0x10);
	    // data[1] = i2c3_get(0x11);


	    if (timeout > 1) {
		timeout--;
	    } else if (timeout == 1) {
		i2c_w_setn(0x20, rgb, 4);
		timeout--;

		uart2_str0("idle\n");
	    }
	    delay_us(100);
	}

	return 0;
}