//Plays the audio backwards by iterating from end of buffer to the beginning
int ReversePlay(data_file file_, int l_, int* clusters_)
{
	int i, j;
	BYTE playBuffer[BUF_SIZE] = {0};

	for (i = l_ * BPB_SecPerClus; i > 0; i--)
	{
		if (_playing == 0)
			break;

		get_rel_sector(&file_, playBuffer, clusters_, i);

		//Starts from the end and works backwards
		for (j = 508; j > 0; j-=6)
		{
			//Wait for signal to write for audio, then write to it with prepared bytes
			while(IORD(AUD_FULL_BASE, 0)){}
			IOWR(AUDIO_0_BASE, 0 , BytePrep(playBuffer[j], playBuffer[j + 1]));

			//To play the next 2 bits of reverse batch (right side)
			j+=2;
			while(IORD(AUD_FULL_BASE, 0)){}
			IOWR(AUDIO_0_BASE, 0 , BytePrep(playBuffer[j], playBuffer[j + 1]));
		}
	}
}

// this function is called immediately upon exiting your main program to softly shut down the 324EGM and report test details
void finalize(void)
{
	int nMissed; // missed events
	int nLatency; // maximum latency (in 1024ths of a period)
	int nLatency_ms; // maximum latency (in milliseconds)

	// immediately disable the 324EGM
	IOWR(PIO_EGMENABLE_BASE, 0, 0);
	IOWR(PIO_RESPONSE_BASE, 0, 1);
	usleep(10);

	// obtain the test results
	nLatency = IORD(PIO_LATENCY_BASE, 0);
	nMissed = IORD(PIO_MISSED_BASE, 0);

	// calculate the latency in milliseconds
	nLatency_ms = nLatency * (g_period + 1) * 3 / 25;

	// print the results
	printf("Test complete.\n");
	printf("Results:\n");
	printf("  missed = %d pulse(s)\n", nMissed);
	printf("  max latency = %d / 1024th(s) of a period\n", nLatency);
	printf("  max latency = %d microsecond(s)\n", nLatency_ms);
	printf("  task units processed = %d units\n\n", g_taskProcessed);
	printf("Exiting...\n");

	// this tells the program that we're done
	// removed to allow looping.
	// printf("\004");
	return;
}

/* Tests basic sigmoid functionality */
int test_sigmoid(void){

	float M,time,result;

	alltypes_u input;

	M = 2.24;
	time = 3.8;
	result = 0;

	fprintf(stderr,"Starting sigmoid test....\n");

	fprintf(stderr,"Sigmoid block test register returns [%X].\n", IORD(EULERBLOCK_0_BASE,3));

	fprintf(stderr,"Floats have byte count of [%d].\n",(int)sizeof(float));

	input.fl = M;

	IOWR(EULERBLOCK_0_BASE,0,input.ui);

	/* Check it's corrent */
	input.ui = IORD(EULERBLOCK_0_BASE, 0);


	fprintf(stderr,"Inputs are M=[%4.2f], time=[%4.2f].\n",M,time);

	fprintf(stderr,"IORD returns [%4.8f].\n", input.fl);

	sigmoid(M, time, &result);

	fprintf(stderr,"Sigmoid returned [%4.8f].\n", result);

	return 0;
}

int DelayPlay(data_file file_, int l_, int* clusters_)
{
	int i, j;

	//Create an additional delay buffer with the size of sample rate
	BYTE playBuffer[BUF_SIZE] = {0};
	UINT16 delayBuffer[SAMPLE_RATE] = {0};

	int idxDelay, flag = 0;

	//Iterate through the audio
	for (i = 0; i < l_ * BPB_SecPerClus; i++)
	{
		if (_playing == 0)
			break;

		get_rel_sector(&file_, playBuffer, clusters_, i);

		for (j = 0; j < BUF_SIZE; j+=2)
		{
			//Wait for signal to write for audio, then write to it with prepared bytes
			while(IORD(AUD_FULL_BASE, 0)){}

			//Populate delayBuffer using idxDelay with what's in playBuffer on an edge
			if (flag == 0){
				IOWR(AUDIO_0_BASE, 0 , BytePrep(playBuffer[j], playBuffer[j + 1]));
				flag = 1;
			}else{
				IOWR(AUDIO_0_BASE, 0, delayBuffer[idxDelay]);
				delayBuffer[idxDelay] = BytePrep(playBuffer[j], playBuffer[j + 1]);
				flag = 0;
			}

			//If we exceed the buffer size, loop around to beginning of array
			idxDelay++;
			if (idxDelay > SAMPLE_RATE)
				idxDelay = idxDelay % SAMPLE_RATE;
		}
	}

	//Finish from current delay index to end of delay array
	for (i = idxDelay; i < SAMPLE_RATE; i++)
	{
		if (_playing == 0)
			break;

		while(IORD(AUD_FULL_BASE, 0)){}
		IOWR(AUDIO_0_BASE, 0, delayBuffer[i]);
	}

	//Finish last remaining audio data from beginning to delayIdx
	for (i = 0; i < idxDelay; i++)
	{
		if (_playing == 0)
			break;

		while(IORD(AUD_FULL_BASE, 0)){}
		IOWR(AUDIO_0_BASE, 0, delayBuffer[i]);
	}
}

USHORT Hal4D13_ReadEndpointWOClearBuffer(UCHAR bEPIndex, UCHAR * buf, USHORT len)
{
    USHORT i, j,c;
    IOWR(USB_0_BASE,D13_COMMAND_PORT, D13CMD_EP_RD_FIFO + bEPIndex);
    /* read Buffer */
    j = IORD(USB_0_BASE,D13_DATA_PORT);
    if(j > len)
        j = len;
    i=0;
    while (i<j) //<<    
   //for(i<j; i=i+2, buf++ )
    {
         c = IORD(USB_0_BASE,D13_DATA_PORT);
         *buf = (UCHAR)c;//printf("WOC= %02X ",*buf);//<<        
         i++;//<<
         if (i >= j) break; //<<
         buf++;
         *buf = (UCHAR)(c>>8); //printf("WOC= %02X ",*buf);//<<
         i++;//<<
     buf++;
    }
   // printf("\n",c);
    /* Clear Buffer */
    IOWR(USB_0_BASE,D13_COMMAND_PORT, D13CMD_EP_CLEAR_BUF+bEPIndex);

    return j;
}

ULONG Hal4D13_GetIntEnable(void)
{
    ULONG i;
    IOWR(USB_0_BASE,D13_COMMAND_PORT,D13CMD_DEV_RD_INTEN);
    i = IORD(USB_0_BASE,D13_DATA_PORT);
    i += (((ULONG)IORD(USB_0_BASE,D13_DATA_PORT)) << 16);
    return i;
}

void init_task(void *pdata)
{
    /* Tells the user that we are up and running. */
    WARNING(0, "System boot !");

#if 0
    /* Inits the custom math lib. */
    NOTICE(0, "Fast math init.");
    fast_math_init();
#endif

    cvra_beacon_init(&robot.beacon, AVOIDING_BASE, AVOIDING_IRQ, 100, 10., 0.);
    cvra_beacon_set_direction_offset(&robot.beacon, 123);



    ip_stack_init();
    list_netifs();

    /* If the logic power supply is off, kindly ask the user to turn it on. */
    if ((IORD(PIO_BASE, 0) & 0xff) == 0) {
        printf("Hey sac a pain, la commande c'est en option ?\n");

        /* We refuse to let the user to a shell before he turns it on. */
        while ((IORD(PIO_BASE, 0) & 0xff) == 0);
        printf("Merci bien !\n");
    }

    /* Inits all the trajectory stuff, PID, odometry, etc... */
#if 1
    NOTICE(0, "Main control system init.");
    cvra_cs_init();
#endif



    /* Sets the bounding box for the avoidance module. */
    const int robot_size = 150;
    polygon_set_boundingbox(robot_size, robot_size, 3000-robot_size, 2000-robot_size);

    arm_highlevel_init();

    lua_do_settings();

    luaconsole_init();

    OSTaskCreateExt(heartbeat_task,
                    NULL,
                    &heartbeat_task_stk[TASK_STACKSIZE-1],
                    HEARTBEAT_TASK_PRIORITY,
                    HEARTBEAT_TASK_PRIORITY,
                    &heartbeat_task_stk[0],
                    TASK_STACKSIZE,
                    NULL, NULL);

    /* Tasks must delete themselves before exiting. */
    OSTaskDel(OS_PRIO_SELF);
}

// return 0 : success
//        1 : fail
int iic_read(
    unsigned char addr, // 7bit-addr
    int num,
    unsigned char *data )
{
    int i, r, err;

    err = 0;

    IOWR( IIC_0_BASE, 0, 0x100 ); // I2C Start
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("S%x\n", r );

    addr = (addr << 1) | 1;

    IOWR( IIC_0_BASE, 0, addr ); // I2C WRITE
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("C%x\n", r );
    if( r & 0x400 ) { // NACK
        err = 1;
        goto end;
    }

    for( i = 0; i < num-1; i++ ) {
        IOWR( IIC_0_BASE, 0, 0x300 ); // I2C READ with ACK
        r = IORD( IIC_0_BASE, 0 );
        while( (r & 0x100) ) {
            r = IORD( IIC_0_BASE, 0 );
        }
//      alt_printf("R%x\n", r );
        data[i] = r;
    }

    IOWR( IIC_0_BASE, 0, 0x301 ); // I2C READ with NACK
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("R%x\n", r );
    data[i] = r;

end:
    IOWR( IIC_0_BASE, 0, 0x200 ); // I2C STOP
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("P%x\n", r );

    return err;
}

ULONG Hal4D13_ReadInterruptRegister(void)
{
    ULONG j,i = 0;
    IOWR(USB_0_BASE,D13_COMMAND_PORT, D13CMD_DEV_INT_SRC);
    i = IORD(USB_0_BASE,D13_DATA_PORT);
    j = IORD(USB_0_BASE,D13_DATA_PORT);
    j = ((j<<16) & 0xffff0000 ) + (i & 0xffff);
    return i;
}

void DEMO_ENCODERS(void) {
    //read from pins like for PWM
    double encode_0_a, encode_1_a; //encode_b;
    double counts_0_new, counts_0_old;
    double counts_1_new, counts_1_old;
    counts_0_old = 0;
    counts_1_old = 0;

    /* Init motor */
    IOWR(PWM_0_BASE, 0, 0);
    IOWR(PWM_1_BASE, 0, 0);
    IOWR(PWM_2_BASE, 0, 0);
    IOWR(PWM_3_BASE, 0, 0);

    IOWR(PWM_0_BASE, 0, 1 * 31250);
    //IOWR(PWM_2_BASE, 0, 0.5 * 31250);

    //64 counts per revolution, @ 100% is 80rev/min
    //H-bridge slows voltage output, motors see around 8V max
    //(64 counts per revolution of motor shaft) * (131 revolutions of motor shaft per revolutions of output shaft) * (1 revolution of output shaft per 2*PI*4 distance)
    // = 210712.9025
    //    60 / (duty * 80)
/// 43 increments (12V), 1.3333 revs per second
    /*
    	counting only the rising edges of channel A = half of counting the rising/falling edges of channel A+B
    	So 64 counts => 32 counts per revolution
    */

    while (1)
    {
        //1st motor
        counts_0_new = ((double) IORD(ENCODER_0A_BASE, 0)) / 32;
        encode_0_a = (counts_0_new-counts_0_old);
        //encode_b = (((double) IORD(ENCODER_0B_BASE, 0)) / 32) - encode_b;
        if (encode_0_a < 0) {
            encode_0_a = counts_0_new;
        }
        counts_0_old = counts_0_new;

        //2nd motor
        counts_1_new = ((double) IORD(ENCODER_1A_BASE, 0)) / 32;
        encode_1_a = (counts_1_new-counts_1_old);
        if (encode_1_a < 0) {
            encode_1_a = counts_1_new;
        }
        counts_1_old = counts_1_new;

        printf("Motor 0 encoder A value is :( %f ) \n", encode_0_a);
        printf("Motor 0 revolutions per second :( %f ) \n", encode_0_a / 64);
        printf("Motor 1 encoder A value is :( %f ) \n", encode_1_a);
        printf("Motor 1 revolutions per second :( %f ) \n", encode_1_a / 64);

        //printf("Current encoder B value is :( %f ) \n", encode_b / 32);
        usleep(1000000);
    }
}

static void pulse_ISR(void* context, alt_u32 id)
{
	   if(IORD(PIO_PULSE_BASE, 0) == 0 && IORD(PIO_RESPONSE_BASE, 0) == 1){
	   		IOWR(PIO_RESPONSE_BASE, 0, 0);
	   		eventCounterPulse++;
	   }
	   if(IORD(PIO_PULSE_BASE, 0) == 1 && IORD(PIO_RESPONSE_BASE, 0) == 0){
	      		IOWR(PIO_RESPONSE_BASE, 0, 1);
	   }
	   // acknowledge the interrupt by clearing the TO bit in the status register
	   IOWR(PIO_PULSE_BASE, 3, 0x0);
}

/* A normalized sigmoid - result is always 0-1
 * M is a time value which must be the same unit as the "time" parameter and represents
 * the half-time point*/
int sigmoid(float M, float time, float*result) {

	alltypes_u input,output;

	/* Semaphore is normally created by the movement init function.. */
	if(xSemaphore == NULL){
		xSemaphore = xSemaphoreCreateMutex();
	}

	/* The M value is the half time point where the gradient is maximum */

	/* Boundary checks ? */

	/* Do the sigmoid calcs */
	input.fl = (-1 * (SIGMOID_ERR) * (time - M)) / M;

	if (xSemaphore != NULL) {
		// See if we can obtain the semaphore.  If the semaphore is not available
		// wait 10 ticks to see if it becomes free.
		if (xSemaphoreTake( xSemaphore, ( portTickType ) 10 ) == pdTRUE) {

			/* Write to block and wait for the euler block to become ready -
			 * this is known at 17 cycles. So
			 * really we should just run 17nops. Remember a CS takes many more ! So let's
			 * give it the benefit of the doubt and just run nop if we can't use it. If all the
			 * other servos are trying to use it right at this moment then maybe we've got problems.
			 * We could make this a CRITICAL section...*/

			IOWR(EULERBLOCK_0_BASE,0,input.ui);

			while (IORD(EULERBLOCK_0_BASE,2)==0);

			output.ui = IORD(EULERBLOCK_0_BASE, 1);

			xSemaphoreGive( xSemaphore);

		} else {
			fprintf(stderr,"Servo couldn't get EULERBLOCK semaphore! \n");
			// We could not obtain the semaphore and can therefore not access
			// the shared resource safely.
		}

	}

	else{
		fprintf(stderr,"ERROR: Sigmoid sempahore is NULL. Returning.\n");
		return ECD_ERROR;
		}

	*result = 1.0/(1 + output.fl);

	return ECD_OK;
}

unsigned short readPixel(int col, int row, int region)
{
    int address = (row<<8) + col;
    if(region == 0)
        return IORD(TL_BASE, address);
    else if(region == 1)
        return IORD(TR_BASE, address);
    else if(region == 2)
        return IORD(BL_BASE, address);
    else if(region == 3)
        return IORD(BR_BASE, address);    
    return 0;
}

static void timer_ISR(void* context, alt_u32 id)
{
   // acknowledge the interrupt by clearing the TO bit in the status register
   IOWR(TIMER_1_BASE, 0, 0x0);
   // only write the current value to the RESPONSE_BASE when the values of PULSE_BASE and RESPONSE_BASE are different
   if(IORD(PIO_PULSE_BASE, 0) == 0 && IORD(PIO_RESPONSE_BASE, 0) == 1){
   		IOWR(PIO_RESPONSE_BASE, 0, 0);
   		eventCounter++;
   }
   if(IORD(PIO_PULSE_BASE, 0) == 1 && IORD(PIO_RESPONSE_BASE, 0) == 0){
      		IOWR(PIO_RESPONSE_BASE, 0, 1);
   }
}

/* MMCから1バイト受信 */
alt_u8 mmc_spi_Recvbyte(void)
{
	alt_u32 res;

	while( !(IORD(mmc_spi_reg, mmcreg_status) & mmc_commexit) ) {}
	IOWR(mmc_spi_reg, mmcreg_status, (mmc_commstart | mmc_spi_ncs | 0xff));

	do {
		res = IORD(mmc_spi_reg, mmcreg_status);
	} while( !(res & mmc_commexit) );

	return (alt_u8)(res & 0xff);
}

void n2h2_isr(void* context)
{
  N2H_isr_fifo* fifo = (N2H_isr_fifo*) context;  

  // Read the cause of the interrupt
  int interrupter = IORD(N2H2_CHAN_BASE, 7);

  
  if((0x80000000 & interrupter) != 0)
    { 
      N2H_isr_info* info = (N2H_isr_info*) malloc(sizeof(N2H_isr_info));
      info->isr_type = RX_UNKNOWN;

      // Read in incoming hibi address
      info->dst_address = IORD(N2H2_CHAN_BASE, 12);      
      // Clear IRQ
      IOWR(N2H2_CHAN_BASE, 7, 0x80000000); 
      
      // Store interrupt information to fifo
      n2h_isr_fifo_push(fifo, info);    
    }
  
  if((0x40000000 & interrupter) != 0)
    {
      N2H_isr_info* info = (N2H_isr_info*) malloc(sizeof(N2H_isr_info));
      info->isr_type = TX_IGNORED;
      
      // Clear IRQ
      IOWR(N2H2_CHAN_BASE, 7, 0x40000000); 

      // Store interrupt information to fifo
      n2h_isr_fifo_push(fifo, info);    
    }
  
  while((0x3FFFFFFF & interrupter) != 0)
    {
      N2H_isr_info* info = (N2H_isr_info*) malloc(sizeof(N2H_isr_info));
      info->isr_type = RX_READY;
      
      // Store interrupted channel
      info->rx_channel = onehot2int(interrupter);
      // Clear IRQ
      IOWR(N2H2_CHAN_BASE, 7, (1 << info->rx_channel));

      interrupter = interrupter & ~(1 << info->rx_channel);

      // Store interrupt information to fifo
      n2h_isr_fifo_push(fifo, info);    
    }  
}

static void timer_ISR(void* context, alt_u32 id)
{
	// acknowledge the interrupt by clearing the TO bit in the status register
   IOWR(TIMER_1_BASE, 0, 0x0);
   if(IORD(PIO_PULSE_BASE, 0) == 0){
		IOWR(PIO_RESPONSE_BASE, 0, 1);
		flag++;
   }
   if(IORD(PIO_PULSE_BASE, 0) == 1){
	   IOWR(PIO_RESPONSE_BASE, 0, 0);
	   flag++;
   }
   IOWR(TIMER_1_BASE, 1, 0x8);
}

void write_to_codec_double(BYTE * buffer, int sectorSize)
{
	UINT16 tmp;
	int i;

	for (i = 0; (i+4) < sectorSize; i+=8){
		while(IORD(AUD_FULL_BASE,0)) {} // Wait until FIFO is ready
		tmp = (buffer[i+1] << 8) | (buffer[i]);
		IOWR(AUDIO_0_BASE, 0 , tmp);
		while(IORD(AUD_FULL_BASE,0)) {} // Wait until FIFO is ready
		tmp = (buffer[i+3] << 8) | (buffer[i+2]);
		IOWR(AUDIO_0_BASE, 0 , tmp);
	}
}

int main() {

	volatile unsigned char buf[512];

	int status = IORD(SD_CARD_0_BASE, 0);

	printf("SD card status: %d\n", status);

	memset(buf, 0, sizeof(buf));

	IOWR(SD_CARD_0_BASE, 0, buf);
	IOWR(SD_CARD_0_BASE, 1, 0);
	IOWR(SD_CARD_0_BASE, 2, 1);
	IOWR(SD_CARD_0_BASE, 3, 2); //read

	while(1) {
		status = IORD(SD_CARD_0_BASE, 0);
		printf("SD card status for read: %d\n", status);

		if(status == 2) break;
	}

	int i;
	for(i=0; i<512; i++) {
		if(i > 0 && (i%32) == 0) printf("\n");

		printf("%02x ", buf[i]);
	}
	printf("\n");

	for(i=0; i<512; i++) if(buf[i] != (unsigned char)i) printf("Not Equal: %d\n", i);

	for(i=0; i<512; i++) buf[i] = i;

	IOWR(SD_CARD_0_BASE, 0, buf);
	IOWR(SD_CARD_0_BASE, 1, 0);
	IOWR(SD_CARD_0_BASE, 2, 1);
	IOWR(SD_CARD_0_BASE, 3, 3); //write

	while(1) {
		status = IORD(SD_CARD_0_BASE, 0);
		printf("SD card status for write: %d\n", status);

		if(status == 2) break;
	}


	return 0;
}

bool TMEM_QuickVerify(alt_u32 BaseAddr, alt_u32 DataSize, alt_u32 DataWidth, alt_u32 AddrWidth){
    bool bPass = TRUE;
    const alt_u32 TestNum = 1024*512;
    const alt_u32 TestPattern = 0xAA;
    alt_u32 mask, Read32, Addr32, TestData32, TestAddr32;
    int i;
    
    //alt_u32 *pMem = (alt_u32 *)BaseAddr;
    // test address line
    mask = 0x01;
    for(i=0;i<AddrWidth && bPass;i++){
        //*(pMem + mask) = TestPattern;
        IOWR(BaseAddr, mask, TestPattern);
        //if (*(pMem + mask) != TestPattern)
        Read32 = IORD(BaseAddr, mask);
        if (Read32 != TestPattern)
            bPass = FALSE;
        mask <<= 1;    
    }
    
    // test data line
    mask = 0x01;
    for(i=0;i<DataWidth && bPass;i++){
        //*(pMem+i/32) = mask;
        Addr32 = i*13;
        IOWR(BaseAddr, Addr32, mask);
        Read32 = IORD(BaseAddr, Addr32);
        //if (*(pMem+i/32) != mask)
        if (Read32 != mask)
            bPass = FALSE;
        mask <<= 1;
        if (mask == 0x00)
            mask = 0x01;    
    }
    
    // random data test
    srand(alt_nticks());
    for(i=0;i<TestNum && bPass;i++){
        TestAddr32 = rand()%(DataSize/4);
        TestData32 = rand();
        IOWR(BaseAddr, TestAddr32, TestData32);
        Read32 = IORD(BaseAddr, TestAddr32);
        if (Read32 != TestData32)
            bPass = FALSE;        
        
    }
    
    return bPass;
}