#include "mmb0.h"
#include "usb.h"
#include "ads1271.h"
#include "bfifo.h"
#include "clk.h"
#include <stdlib.h>
#include <c55.h>
#include <mbx.h>

#define MSG_STATE	0
#define MSG_ADCSTT	2
#define MSG_ADCSTP	3
#define MSG_RDATA	4
#define MSG_RDSTOP	6
#define MSG_AVAIL	7
#define MSG_BUFLEN	9
#define MSG_BLOCKSIZE	10
#define MSG_CMODE	11
#define MSG_SRATE	0x10
#define MSG_TSTRAM	0xF0
#define MSG_LEDSW	0xF1
#define MSG_TSTFLASH	0xF7

// byte addresses
#define SDRAM_START 0x40000
#define SDRAM_END 0xff0000

#define STATE_IDLE 0
#define STATE_ADCSTT 1
#define STATE_ADCRUN 2
#define STATE_ADCSTP 3
#define STATE_RDATA 4
#define STATE_TSTRAM 5
#define STATE_TSTFLASH 8
#define STATE_ADSERR 0x86

#define RAMTEST_UNTESTED 0
#define RAMTEST_PASSED 1
#define RAMTEST_WRITING 2
#define RAMTEST_FAILED 3
#define RAMTEST_READING 4

typedef struct msg_t {
	u16 msg;
	u32 arg1;
} msg_t;

//static MBX_Handle ctl_mbx, acq_mbx;

static usb_data_t usbtxbuf[32];

static volatile struct {
	unsigned int ramtest_stat:3,
		initted:1,
		flashtest_stat:2;
	u16 ramtest_pages;
	u32 chmap;
	u16 blocksize; // samples in a block
	int state;
	float clk;
	// blockbytes = length of a packed packet in bytes
	u32 buf1adr, buf2adr, blockbytes; // acq buffers
	u32 acqbytes; // length of acq buffer in bytes
} flags;

static bfifo_t adsfifo;

void delay(u32 c);

static void longalign(u32 *adr)
{
	if (*adr&3) {
		*adr&=~((unsigned long)(3));
		*adr+=4;
	}
}

static void state_set(int state)
{
	char c;

	flags.state=state;
	switch(state) {
	case STATE_IDLE:
		c='i';
		break;
	case STATE_ADCSTT:
		c='g';
		break;
	case STATE_ADCRUN:
		c='c';
		break;
	case STATE_ADCSTP:
		c='s';
		break;
	case STATE_RDATA:
		c='r';
		break;
	case STATE_TSTRAM:
		c='t';
		break;
	default:
		if (state&0x80)
			c='e';
		else
			c='-';
		break;
	}
	led_showchar(c);
}

static int state_get(void)
{
	int st=flags.state;

	if (st&0x80)
		state_set(STATE_IDLE);
	return st;
}

static int countones(u32 chmap)
{
	int i,fcnt;
	
	fcnt=0;
	for (i=0;i<32;++i) {
		if (chmap&1) ++fcnt;
		chmap=chmap>>1;
	}
	return fcnt;
}

static u32 usbU32(usb_data_t *buf)
{
	u32 d;
	
	d=(u32)(*buf)<<16;
	d|=buf[1];
	return d;
}

static void usbPutU32(usb_data_t *buf, u32 arg)
{
	buf[0]=(arg>>16)&0xffff;
	buf[1]=(arg&0xffff);
}

/* ------ RAM test */

static void ramtest_write(int page, int inc)
{
	unsigned long loc=(unsigned long)page<<16;
	unsigned long end=loc+0x10000;

	while (loc<end) {
		*(unsigned int *)(loc)=(unsigned int)((loc*inc)&0xfffful);
		++loc;
	}
}

static int ramtest_read(int page, int inc)
{
	unsigned long loc=(unsigned long)page<<16;
	unsigned long end=loc+0x10000;
	int err=0;
	unsigned int read, calc;

	while (loc<end) {
		calc=(unsigned int)((loc*inc)&0xfffful);
		read=*((unsigned int *)(loc))&0xfffful;
		if (read!=calc) {
			++err;
			led_dp(1);
			led_showdig(err);
		}
		++loc;
	}
	return err;
}

static int ramtest_exec(void)
{
	led_dp(flags.ramtest_pages&1);
	if (flags.ramtest_stat==RAMTEST_READING) {
		if (ramtest_read(flags.ramtest_pages+4,7)) {
			flags.ramtest_stat=RAMTEST_FAILED;
			state_set(STATE_IDLE);
			return 0;
		}
		++flags.ramtest_pages;
		if (flags.ramtest_pages>(0x7e)-4) {
			flags.ramtest_stat=RAMTEST_PASSED;
			state_set(STATE_IDLE);
			return 0;
		}
	} else {
		ramtest_write(flags.ramtest_pages+4,7);
		++flags.ramtest_pages;
		if (flags.ramtest_pages>(0x7e)-4) {
			flags.ramtest_pages=0;
			flags.ramtest_stat=RAMTEST_READING;
		}
	}
	return 1;
}

static void ramtest(void)
{
	if (flags.state!=STATE_IDLE) return;
	state_set(STATE_TSTRAM);
	flags.ramtest_pages=0;
	flags.ramtest_stat=RAMTEST_WRITING;
	while(ramtest_exec());
}

static void testflash(void)
{
	if (flash_test())
		flags.flashtest_stat=2;
	else
		flags.flashtest_stat=1;
}

/* ------ Data acq thread

Mailbox codes:

MSG_ADCSTT = start ADC with current conv parms: block size, channel map, and whatever 
  else can't be changed while the ADC is running
MSG_ADCSTP = stop ADC.  Ignored if idle.  This will cancel a running acq.
MSG_RDATA = acquire n blocks.  N kept in convblocks.  Starts ADC if this isn't done
MSG_RDSTOP = cancel acquisition.  Only checked at FIFO or USB boundaries, so may 
  take time to take effect.  Switches off USB DMA immediately.
*/

void acq_rdata(u32 convlen)
{
	u32 i,rloc;
	
	state_set(STATE_RDATA);
	bfifo_clear(&adsfifo);
	ads_pack_start(&adsfifo,convlen);
	for (i=0;i<convlen;++i) {
		// ### TODO timeout on fifo not empty
		while (bfifo_isempty(&adsfifo));
		// ### TODO check for message here
		rloc=bfifo_rstart(&adsfifo);
		// ### TODO timeout on usb bulk tx
		while(usb_tx(17,(usb_data_t *)(rloc>>1),flags.blockbytes)==-3);
	}
	ads_pack_stop();
	// ### TODO timeout on pack stop
	while (ads_packing());
}

void acq_adcstt(void)
{
	msg_t m;

	state_set(STATE_ADCSTT);
	ads_acquire_start(flags.blocksize,2,flags.buf1adr,flags.buf2adr); // FIXME: assuming 2 channels
	while (!ads_acquiring());
	
	for(;;) {
		state_set(STATE_ADCRUN);
		MBX_pend(&acq_mbx,&m,SYS_FOREVER);
		switch(m.msg) {
		case MSG_RDATA:
			acq_rdata(m.arg1);
			break;
		case MSG_ADCSTP:
			state_set(STATE_ADCSTP);
			ads_acquire_stop();
			while (ads_acquiring());
			break;
		default:
			break;
		}
		if (flags.state==STATE_ADCSTP) break;
	}
}

void acq_thread(void)
{
	msg_t m;

	for(;;) {
		state_set(STATE_IDLE);
		MBX_pend(&acq_mbx,&m,SYS_FOREVER);
		switch(m.msg) {
		case MSG_ADCSTT:
			acq_adcstt();
			break;
		default:
			break;
		}
	}
}

static void msg_post(MBX_Handle mbx, u16 msg, u32 arg1)
{
	msg_t m;

	m.msg=msg;
	m.arg1=arg1;
	MBX_post(mbx,&m,0);
}

static void state(int state)
{
	//if (!acq_mbx) acq_mbx=MBX_create(sizeof(msg_t),8,0);
	//if (!ctl_mbx) ctl_mbx=MBX_create(sizeof(msg_t),8,0);
	if (state!=USB_STATE_CONFIGURED) return;
	//usb_set_txdone_cb(9,txdone);
	ads_sync();
}

// rate is in millihertz
static void srate_set(long rate)
{
	float frate=(float)rate;

	if (ads_conv_mode()==ADS_CONV_MODE_HS)
		frate*=0.256;
	else
		frate*=0.512;
	if (frate>29e6) frate=28e6;
	if (frate<3e6) frate=3e6;
	flags.clk=clk_set_outfreq(CLK_PIN_B,2,frate);
	delay(200);
	ads_sync();
	delay(200);
}

static float srate_get(void)
{
	if (ads_conv_mode()==ADS_CONV_MODE_HS)
		return flags.clk/0.256;
	else
		return flags.clk/0.512;
}

static int cmode_get(void)
{
	return ads_conv_mode();
}

static void blocksize_set(long size)
{
	u32 a,blocks,fifoadr;

	if (flags.state!=STATE_IDLE) return;
	
	flags.blocksize=size;
	a=countones(flags.chmap)*size;
	flags.acqbytes=a*4;
	flags.blockbytes=a*3;
	flags.buf1adr=SDRAM_START;
	a=flags.buf1adr+flags.acqbytes;
	longalign(&a);
	flags.buf2adr=a;
	fifoadr=flags.buf2adr+flags.acqbytes;
	longalign(&fifoadr);
	blocks=(u32)(SDRAM_END)-fifoadr;
	blocks/=flags.blockbytes;
	bfifo_init(&(adsfifo),fifoadr,blocks,flags.blockbytes);
}

/* Control messages
CTL_MSG_CMODE - arg1 = conv mode
CTL_MSG_SRATE - arg1 = sample rate
CTL_MSG_TSTRAM - no args
CTL_MSG_BLOCKSIZE - arg1 = block size
CTL_MSG_LEDSW - arg1 = new LED state
*/

void ctl_thread(void)
{
	msg_t m;

	for(;;) {
		MBX_pend(&ctl_mbx,&m,SYS_FOREVER);
		switch(m.msg) {
		case MSG_CMODE:
			ads_set_conv_mode(m.arg1);
			break;
		case MSG_SRATE:
			srate_set(m.arg1);
			break;
		case MSG_TSTRAM:
			if (state_get()!=STATE_IDLE)
				return;
			ramtest();
			break;
		case MSG_BLOCKSIZE:
			if (state_get()!=STATE_IDLE)
				return;
			blocksize_set(m.arg1);
			break;
		case MSG_LEDSW:
			led_set(m.arg1);
			break;
		case MSG_TSTFLASH:
			testflash();
			break;
		//case MSG_WFLASH:
		//case MSG_RFLASH:
		default:
			break;
		}
	}
}

static void ctl_write(void)
{
	int err;

	err=0;
	switch(usb_setup.request) {
	case MSG_ADCSTT:
		if (state_get()!=STATE_TSTRAM)
			msg_post(&acq_mbx,MSG_ADCSTT,0);
		break;
	case MSG_ADCSTP:
		if (state_get()!=STATE_TSTRAM)
			msg_post(&acq_mbx,MSG_ADCSTP,0);
		break;
	case MSG_RDATA:
		if (state_get()!=STATE_TSTRAM)
			msg_post(&acq_mbx,MSG_RDATA,(long)usbU32(usb_ctl_write_data));
		break;
	case MSG_CMODE:
		msg_post(&ctl_mbx,MSG_CMODE,usb_setup.value);
		break;
	case MSG_SRATE:
		msg_post(&ctl_mbx,MSG_SRATE,usbU32(usb_ctl_write_data));
		break;
	case MSG_BLOCKSIZE:
		if (state_get()==STATE_IDLE)
			msg_post(&ctl_mbx,MSG_BLOCKSIZE,usb_setup.value);
		break;
	case MSG_TSTRAM:
		if (state_get()==STATE_IDLE)
			msg_post(&ctl_mbx,MSG_TSTRAM,0);
		break;
	case MSG_TSTFLASH:
		if (state_get()==STATE_IDLE)
			msg_post(&ctl_mbx,MSG_TSTFLASH,0);
	case MSG_LEDSW:
		msg_post(&ctl_mbx,MSG_LEDSW,usb_setup.value);
		break;
	default:
		err=-1;
	}
	if (err)
		usb_ctl_write_end(1);
	else
		usb_ctl_write_end(0);
}

static void ctl_read(void)
{
	u32 a;

	switch(usb_setup.request) {
	case MSG_AVAIL:
		//bfifo_lock();
		a=bfifo_ravail(&adsfifo);
		if (bfifo_isreading(&adsfifo))
			++a;
		//bfifo_unlock();
		usbPutU32(usbtxbuf,a);
		usb_ctl_read_end(0,4,usbtxbuf);
		break;
	case MSG_BLOCKSIZE:
		usbtxbuf[0]=flags.blocksize;
		usb_ctl_read_end(0,2,usbtxbuf);
		break;
	case MSG_SRATE:
		usbPutU32(usbtxbuf,(u32)srate_get());
		usb_ctl_read_end(0,4,usbtxbuf);
		break;
	case MSG_CMODE:
		usbtxbuf[0]=cmode_get()<<8;
		usb_ctl_read_end(0,1,usbtxbuf);
		break;
	case MSG_STATE:
		usbtxbuf[0]=(state_get()&0xff)<<8;
		usb_ctl_read_end(0,1,usbtxbuf);
		break;
	case MSG_BUFLEN:
		a=flags.blockbytes; // packet length in bytes
		a&=~3; // round up to longword
		a+=4;
		usbPutU32(usbtxbuf,(long)((SDRAM_END-SDRAM_START)/a));
		usb_ctl_read_end(0,4,usbtxbuf);
		break;
	case MSG_TSTRAM:
		usbtxbuf[0]=flags.ramtest_stat<<8;
		usbtxbuf[0]|=(u8)((flags.ramtest_pages>>8)&0xff);
		usbtxbuf[1]=(flags.ramtest_pages&0xff)<<8;
		usb_ctl_read_end(0,3,usbtxbuf);
		break;
	case MSG_LEDSW:
		usbtxbuf[0]=led()<<8;
		usbtxbuf[0]|=sw_read();
		usb_ctl_read_end(0,2,usbtxbuf);
		break;
	case MSG_TSTFLASH:
		usbtxbuf[0]=flags.flashtest_stat<<8;
		usb_ctl_read_end(0,1,usbtxbuf);
		break;
	default:
		usb_ctl_read_end(1,0,0);
		break;
	}
}

void handle_ctl(void)
{
	int err;
	
	err=usb_ctl_std();
	if (!err) {
		if (usb_setup.dataDir)
			ctl_read();
		else
			ctl_write();
	}
}

void usb_ctl(void)
{
	SWI_post(&swi_ctl);
}

void main(void)
{
	//acq_mbx=ctl_mbx=0;
	mmb0_init(144000000);
	flash_init();
	usb_init(0);
	flags.state=STATE_IDLE;
	led_showchar('i');
	flags.chmap=3;
	flags.ramtest_stat=RAMTEST_UNTESTED;
	flags.flashtest_stat=0;
	blocksize_set(2048);
	C55_enableIER1(C55_IEN018); // dma channel 0
	ads_init();
	srate_set(100000000L);
	ads_set_conv_mode(ADS_CONV_MODE_HR);
	usb_set_state_cb(state);
	usb_attach();
}

// ### TODO (long term) Split common code from generic code ..