#include <wixel.h>
#include <usb.h>
#include <usb_com.h>
#include <stdio.h>
#include <hal.h>

#define MAX_BUF_SZ      64
#define DATA_AQU_TIME   102
#define COOLOFF_PERIOD  1000

#define lowByte(v)   ((unsigned char) (v))
#define highByte(v)  ((unsigned char) (((unsigned int) (v)) >> 8))

uint16 XDATA adcOutputX[MAX_BUF_SZ];
uint16 XDATA adcOutputY[MAX_BUF_SZ];
uint16 XDATA adcOutputZ[MAX_BUF_SZ];

uint32 DATA volatile captureWindow = 0;

uint8 XDATA report[1024];
uint16 DATA reportLength = 0;
uint16 DATA reportBytesSent = 0;

uint16 DATA volatile timer1Count = 0;
uint16 DATA volatile dmaIntCountX = 0;
uint16 DATA volatile dmaIntCountY = 0;
uint16 DATA volatile dmaIntCountZ = 0;

int32 CODE param_input_mode = 1;
int32 CODE param_peripheral_mode = 1;
int32 CODE param_bar_graph = 0;

void adcBufferInit(void)
{
    int ii;

    for (ii = 0; ii < MAX_BUF_SZ; ii++)
        adcOutputX[ii] = adcOutputY[ii] = adcOutputZ[ii] = 0;

}

void initPort0GPIO(void)
{
   P0SEL = 0x00;
   P0DIR = ~0x07;		// 0 is input 1 is output (first three bits 0)
   P0INP = 0x00;		// pull-high/low by default P0INP is 0
   if (param_input_mode == -1)
       P2INP = (P2INP | (1<<5));	// PDUP0 = -1: Pull down 
   if (param_input_mode == 1)
       P2INP = (P2INP & ~(1<<5));	// PDUP0 = 1: Pull high
}

void initP0AsPeriphInp(void) __reentrant
{	
  P0INP = 0x3F;		  // Disable pull-ups and pull-downs
  P0SEL = 0x07;
  				 // Select rest as Gen purp O/P  CX
      				 // No need to pull-up or pull-down when
				 // configured as O/P
				 // Here, we are setting rest of P0 ports
				 // as GPIO, when selecting dir P0DIR
				 // select it as o/p pins
      				 // Dont leave them undefined
				 // refer to page 87 sec 12.4.2
  P0DIR = ~(0x07); // Set P0_0, P0_1 and P0_2 to be input
}


void loadDmaController(void)
{
    EA = 0;
    DMAIE = 0;				// Enable DMA Interrupt Mask
    dmaInit();
    // Use 15 NOPs to load configuration to DMA controller
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;

    IEN1 = (IEN1 | 0x01);    		//enable DMA Interrupt Mask
    DMAIE = 1;				// Enable DMA Interrupt Mask
    EA = 1;
}

void armDma() __reentrant
{
    DMAIE = 0;

    //DMAARM = (0x80 | (DMA_CHANNEL_2 | DMA_CHANNEL_3 | DMA_CHANNEL_4));  
    DMAARM = (0x80 );  
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;

    DMAARM = ((DMA_CHANNEL_2 | DMA_CHANNEL_3 | DMA_CHANNEL_4));  
    // Use 9 NOPs to load configuration to DMA controller
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;

    DMAIE = 1;
}

void disarmDma() __reentrant
{
    //DMAARM = (0x80 & (DMA_CHANNEL_2 | DMA_CHANNEL_3 | DMA_CHANNEL_4));  
    // Use 9 NOPs to load configuration to DMA controller

    DMAIE = 0;

    DMAARM = (~(DMA_CHANNEL_2 | DMA_CHANNEL_3 | DMA_CHANNEL_4));  
    // Use 9 NOPs to load configuration to DMA controller
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
}

void initAdc(void) 
{
    ADCIF = 0;
    ADCCFG = 0x07;        // Enable ADC input AIN0(P0_0), AIN1(P0_1), AIN2(P0_2)
    ADCCON2 = 0xB2;       // Set reference voltage (AVDD pin), 
  			  // decimation rate to 512 (12 bit resolution) and 
			  // Sequence is AIN0 to AIN2 
    ADCCON1 = 0x33;       // Set ADC start condition to '11' (ADCCON1.ST = 1)
    ADCCON3 = 0x00;       // Disable extra conversion
}


void startADCConversion(void) __reentrant
{
  ADCCON1 = (ADCCON1 | (1 << 6)); //Start ADC Conversion 
}

void delay9ClkCycles()
{
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
    __asm nop __endasm; __asm nop __endasm; __asm nop __endasm;
}

volatile unsigned char sizeL = 0;
volatile unsigned char sizeH = 0;

void configureDma()
{

    sizeL = lowByte(MAX_BUF_SZ);
    sizeH = highByte(MAX_BUF_SZ);

    // Initialize the three axis buffers and be done.
    // X AXIS
    //First 5 bits of DC6 selects DMA trigger
    // 20 ADC EOC in a seq sample rdy
    // 21 ADC EOC ch 0 in seq smpl rdy
    // 22 ADC EOC ch 1 in seq smpl rdy
    // 23 ADC EOC ch 2 in seq smpl rdy
    // Last 3 bits of DC6:
    // 0b110
    //
    // NOTE: I HAVE USED numbers for individual channels as we also have
    // the radio channel to work with for transmit.
    //

    dmaConfig._2.SRCADDRH = XDATA_SFR_ADDRESS(ADC) >> 8;
    dmaConfig._2.SRCADDRL = XDATA_SFR_ADDRESS(ADC);
    dmaConfig._2.DESTADDRH = ((uint16)adcOutputX >> 8);
    dmaConfig._2.DESTADDRL = (uint16)adcOutputX ;
    dmaConfig._2.VLEN_LENH = (sizeH & 0b00011111);
    dmaConfig._2.LENL = sizeL;
    dmaConfig._2.DC6 = 0b11010101;	// Bit 4:0 Trigger on 21,
    					// Bit 7 -- Word
					// Bit 6:5 - 00 Single, 10 Repeat Single
    dmaConfig._2.DC7 = 0b00011010;	// IRQ (bit 3) is enabled
    					// Offset 7 Bit 5:4 01 destinc(1 word)
					// priority bits 1:0 
    // Y AXIS
    dmaConfig._3.SRCADDRH = XDATA_SFR_ADDRESS(ADC) >> 8;
    dmaConfig._3.SRCADDRL = XDATA_SFR_ADDRESS(ADC);
    dmaConfig._3.DESTADDRH = ((uint16)adcOutputY >> 8);
    dmaConfig._3.DESTADDRL = (uint16)adcOutputY;
    dmaConfig._3.VLEN_LENH = (sizeH & 0b00011111);
    dmaConfig._3.LENL = sizeL;
    dmaConfig._3.DC6 = 0b11010110;	// Bit 4:0 Trigger on 22
    					// Bit 7 -- Word
					// Bit 6:5 - 00 Single, 10 Repeat Single
    					// Can be changed to PREV Channel
    dmaConfig._3.DC7 = 0b00011010;	// IRQ (bit 3) is enabled
    
    // Z AXIS
    dmaConfig._4.SRCADDRH = XDATA_SFR_ADDRESS(ADC) >> 8;
    dmaConfig._4.SRCADDRL = XDATA_SFR_ADDRESS(ADC);
    dmaConfig._4.DESTADDRH = ((uint16)adcOutputZ >> 8);
    dmaConfig._4.DESTADDRL = (uint16)adcOutputZ;
    dmaConfig._4.VLEN_LENH = (sizeH & 0b00011111);
    dmaConfig._4.LENL = sizeL;
    dmaConfig._4.DC6 = 0b11010111;	// Bit 4:0 Trigger on 23
    					// Bit 7 -- Word
					// Bit 6:5 - 00 Single, 10 Repeat Single
    					// Can be changed to PREV Channel
    dmaConfig._4.DC7 = 0b00011010;	// IRQ (bit 3) is enabled
}

void initDSM(void)
{
    P1SEL = 0x02;
    PERCFG = 0x40;
    P1DIR = 0x02;
    T1CTL = T1CTL & ~0x03; // MODE[1:0] Operation is susptended
    T1CNTL = 0x00;
    //T1CC0H = 0x25;	// The number of ticks = 24 * time taken for ADC
    //T1CC0L = 0x20;
    T1CC0H = 0x6F;	// The number of ticks = 24 * time taken for ADC
    T1CC0L = 0x60;
    T1CTL = T1CTL & ~0x10;	// OVFIF Reset to 0
    T1CCTL0 = (T1CCTL0 | 0x04);
    T1CCTL0 = T1CCTL0 & ~T1CCTL0_IM;
    T1CCTL1 = T1CCTL1 & ~T1CCTL1_IM;
    T1CCTL2 = T1CCTL2 & ~T1CCTL2_IM;
    T1CC1L = 0;
    T1CC1H = 0;
    T1CTL = T1CTL | 0x02;	// Modulo Repeatedly from 0 to T1CC0
    T1CCTL1 = T1CCTL1 & ~0x07;
}

void enableDSM(void)
{
    EA = 0;
    T1CNTL = 0x00;
    T1CC1L = 0;
    T1CC1H = 0;
    T1IE = 1;
    OVFIM = 1;
    T1CCTL1 = (T1CCTL1 | 0x38);
    armDma();
    EA = 1;
}

void disableDSM(void)
{
    EA = 0;

    T1CTL &= ~0x03;
    T1CCTL1 &= ~0x07;

    OVFIM = 0;
    T1CCTL1 = (T1CCTL1 & ~T1CCTL1_IM);

    T1CCTL1 = T1CCTL1 & ~0x38;
    //IEN1 = IEN1 & ~0b00000010;
    T1IE = 0;
    disarmDma();
    EA = 1;
}

ISR(T1, 0)
{
    setDigitalOutput(14, 1);
    timer1Count++;
    T1CTL = (T1CTL | 0xF0) & ~0x20;  	// Clear CH0IF.
    setDigitalOutput(14, 0);
    startADCConversion(); 
}

static uint32 irconFlagCount = 0;

ISR(DMA,0)
{
    EA =0;
    DMAIE = 0;

    // DMAIRQ flag provides an interrupt when the transfer count is reached
    // There are 5 DMA Channels.
    // We use four. Ch2, Ch3, Ch4 for 3 I/O ports 
    // and Channel 1 for RX and TX radio packets

    if (IRCON & 0x01)
    {
	irconFlagCount++;
    	IRCON = (IRCON & ~0x01); 		// Clear CPU bit 0
    }

    if (DMAIRQ & 0x04)
    {
        dmaIntCountX++;
	DMAIRQ = DMAIRQ & ~0x04;	// X Axis 
        DMAARM = DMAARM | DMA_CHANNEL_2;  
        delay9ClkCycles();
    } 
    if (DMAIRQ & 0x08)
    {
        dmaIntCountY++;
	DMAIRQ = DMAIRQ & ~0x08;	// Y Axis
        DMAARM = (DMAARM | DMA_CHANNEL_3);  
        delay9ClkCycles();
    } 
    if (DMAIRQ & 0x10)
    {
        dmaIntCountZ++;
	DMAIRQ = DMAIRQ & ~0x10;	// Z Axis 
        DMAARM = (DMAARM | DMA_CHANNEL_4);  
        delay9ClkCycles();
    }
    DMAIE = 1;		// Enable DMA Interrupt Mask. Same as IEN1 bit 0
    EA = 1;             // enable all interrupts  
}

void updateLeds()
{
    usbShowStatusWithGreenLed();
    LED_YELLOW(0);
    LED_RED(getMs() >> 9 & 1);
}

void putchar(char c)
{
    report[reportLength] = c;
    reportLength++;
}

// adcResult should be between 0 and 2047 inclusive.
void printBar(const char * name, uint16 adcResult)
{
    uint8 i, width;
    printf("%-4s %4d mV |", name, adcConvertToMillivolts(adcResult));
    width = adcResult >> 5;
    for(i = 0; i < width; i++){ putchar('#'); }
    for(; i < 63; i++){ putchar(' '); }
    putchar('|');
    putchar('\r');
    putchar('\n');
}
static uint8 cooloffTrue = 0;
static int ii = 0;


void sendReportIfNeeded()
{
    static uint32 lastReport;
    uint8 bytesToSend;
    uint8 j;

    // Create reports.
    if (getMs() - lastReport >= 2000 && reportLength == 0)
    {
        lastReport = getMs();
        reportBytesSent = 0;

	for (j=0 ; cooloffTrue && j < 10;ii++, j++)
	{
		if (ii == MAX_BUF_SZ) 
		{
		   ii = 0;
		   break;
		}
	        printf("\t%8d\t%8d\t%8d\t%8d\t%8d\t%8d\t%8d\t\r\n",
		       timer1Count,
		       dmaIntCountX,
		       dmaIntCountY,
		       dmaIntCountZ,
		       (adcOutputX[ii] & 0x8000) ? 0 : (adcOutputX[ii] >> 4),
		       (adcOutputY[ii] & 0x8000) ? 0 : (adcOutputY[ii] >> 4),
		       (adcOutputZ[ii] & 0x8000) ? 0 : (adcOutputZ[ii] >> 4));
	}
    }

    // Send the report to USB in chunks.
    if (reportLength > 0)
    {
        bytesToSend = usbComTxAvailable();
        if (bytesToSend > reportLength - reportBytesSent)
        {
            // Send the last part of the report.
            usbComTxSend(report + reportBytesSent, reportLength - reportBytesSent);
            reportLength = 0;
        }
        else
        {
            usbComTxSend(report + reportBytesSent, bytesToSend);
            reportBytesSent += bytesToSend;
        }
    }
}

void main()
{
    uint32 triggerTime;

    systemInit();
    usbInit();

    adcBufferInit();

    if (param_peripheral_mode)
        initP0AsPeriphInp();
    else
        initPort0GPIO();
    
    initAdc();
    initDSM();

    configureDma();
    loadDmaController(); 
    
    triggerTime = getMs();

    while(1)
    {
        boardService();
        updateLeds();
        usbComService();
        sendReportIfNeeded();

        if (getMs() - triggerTime > COOLOFF_PERIOD)
        {
	    cooloffTrue = 0;
            captureWindow = triggerTime = getMs();
            enableDSM();
        }
	else
	{
	    cooloffTrue = 1;
	}

        if (captureWindow && ((getMs() - captureWindow) > DATA_AQU_TIME))
        {
            disableDSM();
        }
    }
}