#include <stdint.h>

//volatile register uint32_t __R30;  // Output pins 
volatile register uint32_t __R31;  // Input pins 

//#define CTRL 0x22000 //Start of control registers

void adc_init_Uc(void)
{
	__asm__ __volatile__
	(
	//Init ADC CLKDIV
    " LDI32 r8, 0x44E0D04C \n"
    " LDI32 r9, 0x00000000 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
	//Init ADC CTRL register to enable step editing
    " LDI32 r8, 0x44E0D040 \n"
    " LDI32 r9, 0x00000004 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //ts charge delay
    " LDI32 r8, 0x44E0D060 \n"
	" LDI32 r9, 0x00000001 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPCONFIG 1
    " LDI32 r8, 0x44E0D064 \n"
	" LDI32 r9, 0x002A8010 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 1
    " LDI32 r8, 0x44E0D068 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Enable ADC steps //STEPENEBLE mask
    " LDI32 r8, 0x44E0D054 \n"
    " LDI32 r9, 0x0000002 \n"////enable 5 channels
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//disable step editing
    " LDI32 r8, 0x44E0D040 \n"
    " LDI32 r9, 0x00000000 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);
}

void adc_init_I1(void)
{
	__asm__ __volatile__
	(
	//Init ADC CLKDIV
    " LDI32 r8, 0x44E0D04C \n"
    " LDI32 r9, 0x00000000 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
	//Init ADC CTRL register to enable step editing
    " LDI32 r8, 0x44E0D040 \n"
    " LDI32 r9, 0x00000004 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //ts charge delay
    " LDI32 r8, 0x44E0D060 \n"
	" LDI32 r9, 0x00000001 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPCONFIG 1
    " LDI32 r8, 0x44E0D064 \n"
	//" LDI32 r9, 0x00198010 \n" //AVG16
	" LDI32 r9, 0x00198008 \n" //AVG4
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 1
    " LDI32 r8, 0x44E0D068 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Enable ADC steps //STEPENEBLE mask
    " LDI32 r8, 0x44E0D054 \n"
    " LDI32 r9, 0x0000002 \n"////enable 5 channels
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//disable step editing
    " LDI32 r8, 0x44E0D040 \n"
    " LDI32 r9, 0x00000000 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);
}


void adc_init(void)
{
	__asm__ __volatile__
	(
	//Init ADC CLKDIV
    " LDI32 r8, 0x44E0D04C \n"
    " LDI32 r9, 0x00000000 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
	//Init ADC CTRL register to enable step editing
    " LDI32 r8, 0x44E0D040 \n"
    " LDI32 r9, 0x00000004 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //ts charge delay
    " LDI32 r8, 0x44E0D060 \n"
	" LDI32 r9, 0x00000001 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPCONFIG 1
    " LDI32 r8, 0x44E0D064 \n"
	" LDI32 r9, 0x00000010 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 1
    " LDI32 r8, 0x44E0D068 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//Init ADC STEPCONFIG 2
    " LDI32 r8, 0x44E0D06C  \n"
    " LDI32 r9, 0x00110010 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 2
    " LDI32 r8, 0x44E0D070 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//Init ADC STEPCONFIG 3
    " LDI32 r8, 0x44E0D074 \n"
    " LDI32 r9, 0x00198010 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 3
    " LDI32 r8, 0x44E0D078 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//Init ADC STEPCONFIG 4
    " LDI32 r8, 0x44E0D07C \n"
    " LDI32 r9, 0x002A8010 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 4
    " LDI32 r8, 0x44E0D080 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//Init ADC STEPCONFIG 5
    " LDI32 r8, 0x44E0D084 \n"
    " LDI32 r9, 0x00330010 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Init ADC STEPDELAY 5
    " LDI32 r8, 0x44E0D088 \n"
	" LDI32 r9, 0x00000008 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
    //Enable ADC steps //STEPENEBLE mask
    " LDI32 r8, 0x44E0D054 \n"
    " LDI32 r9, 0x0000003E \n"////enable 5 channels
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//disable step editing
    " LDI32 r8, 0x44E0D040 \n"
    " LDI32 r9, 0x00000000 \n"
    " SBBO &r9, r8, 0, 4 \n"
	);
}

void adc_start(void)
{
	__asm__ __volatile__
	(
    //Enable ADC steps //STEPENEBLE mask
    " LDI32 r8, 0x44E0D054 \n"
//    " LDI32 r9, 0x0000003E \n"////enable 5 channels
	" LDI32 r9, 0x00000002 \n"////enable steps mask
    " SBBO &r9, r8, 0, 4 \n"
	);

	__asm__ __volatile__
	(
	//enables ADC 
    " LDI32 r0, 0x44E0D040 \n"
    " LDI32 r1, 0x00000001 \n"
    " SBBO &r1, r0, 0, 4 \n"
	);
}

uint16_t adc_read(register uint32_t x)
{
  /* must be compiled without optimizations */

  /* r2 used for the stack pointer, do not modify */
  /* r3 used for return address, do not modify */
  /* x=r14, y=r15, z=r16 registers used */
  /* r14 used for return values */

  __asm__ __volatile__
  (
   /* read fifo */
 "  LDI32 r1, 0x44e0d100 \n" //ADC_MIO_ADDR + ADC_REG_FIFO0DATA \n"
 "  LBBO &r14, r1, 0, 4 \n"
 "  LDI32 r1, 0x0000ffff \n"
 "  AND r14, r14, r1 \n"
 "  JMP R3.w2 \n"
  );

  return 0;
}

uint16_t adc_fifo_stat(register uint32_t x)
{
  /* must be compiled without optimizations */

  /* r2 used for the stack pointer, do not modify */
  /* r3 used for return address, do not modify */
  /* x=r14, y=r15, z=r16 registers used */
  /* r14 used for return values */

  __asm__ __volatile__
  (

   /* read fifo */
 "  LDI32 r1, 0x44e0d0e4 \n" //ADC_MIO_ADDR + ADC_REG_FIFO0DATA \n"
 "  LBBO &r14, r1, 0, 4 \n"
 "  LDI32 r1, 0x0000007f \n"
 "  AND r14, r14, r1 \n"
 "  JMP R3.w2 \n"
  );

  return 0;
}

void cnt_start(void)
{
	__asm__ __volatile__
	(
	" LDI32 r8, 0x22000 \n" //set r8 for the lbbo
	" LBBO &r9, r8, 0, 4 \n" // Get the control register
	);
	__asm__ __volatile__
	(
	" SET r9, r9, 3 \n" // Set the cycle counter enable
	" SBBO &r9, r8, 0, 4 \n" // Put back to the register to start
	);	
}

void cnt_stop(void)
{
	__asm__ __volatile__
	(
	" LDI32 r8, 0x22000 \n" //set r8 for the lbbo
	" LBBO &r9, r8, 0, 4 \n" // Get the control register
	);
	__asm__ __volatile__
	(
	" CLR r9, r9, 3 \n" // Set the cycle counter enable
	" SBBO &r9, r8, 0, 4 \n" // Put back to the register to start
	);	
}

void cnt_clear(void)
{
	__asm__ __volatile__
	(
	" LDI32 r8, 0x22000 \n" //set r8 for the lbbo
	" LDI32 r9, 0 \n" //set r8 for the lbbo
	" SBBO &r9, r8, 12, 4 \n" // Put back to the register to start
	);	
}

uint32_t cnt_get(register uint32_t x)
{
	__asm__ __volatile__
	(
	" LDI32 r8, 0x22000 \n" //set r8 for the lbbo
	" LBBO &r14, r8, 0xC, 4 \n" // return value
	" JMP R3.w2 \n"
	);
	//unreachable
	return 0;
}

void mem_write_uint32(register uint32_t x, register uint32_t y)
{
  /* i is the absolute offset relative from shared memory start */
  /* write x at shm + i */

  	__asm__ __volatile__
	(
    " SBBO &r15, r14, 0, 4 \n"
	);
}

void ocp_init(void)
{
  /* enable ocp wide access */

  __asm__ __volatile__
  (
   " LBCO &r0, C4, 4, 4 \n"
   " CLR r0, r0, 4 \n"
   " SBCO &r0, C4, 4, 4 \n"
  );
}

void shm_init(void)
{
  /* configure the programmable pointer register for */
  /* PRU0 by setting c28_pointer[15:0] field to 0x0120 */
  /* this will make C28 point to 0x00012000 (PRU shared RAM). */

  /* save r4, r5 */
  __asm__ __volatile__
  (
   " SUB r2, r2, 8 \n"
   " SBBO &r4, r2, 0, 8 \n"
  );

  __asm__ __volatile__
  (
   " LDI32 r4, 0x0120 \n"
   " LDI32 r5, 0x22028 \n"
   " SBBO &r4, r5, 0x00, 4 \n"
  );

  /* restore r4, r5 */
  __asm__ __volatile__
  (
   " LBBO &r4, r2, 0, 8 \n"
   " ADD r2, r2, 8 \n"
  );
}

void shm_write_uint32(register uint32_t i, register uint32_t x)
{
  /* i is the absolute offset relative from shared memory start */
  /* write x at shm + i */

  __asm__ __volatile__
  (
   " SBCO &r15, C28, r14.w0, 4 \n"
  );
}

void shm_write_float(register uint32_t i, register float x)
{
  __asm__ __volatile__
  (
   " SBCO &r15, C28, r14.w0, 4 \n"
  );
}

uint32_t shm_read(register uint32_t i, register uint32_t b)
{
  /* b is the absolute offset relative from shared memory start */
  /* read x at shm + b */
  
  //r14 used for returning value

  __asm__ __volatile__
  (
   " LDI32 r0, 0x000000120 \n"
   " LDI32 r1, 0x22028 \n"
   " SBBO &r0, r1, 0, 4 \n"
   " LDI32 r0, 0x00100000 \n"

   " LDI32 r1, 0x2202c \n"
   " SBBO &r0, r1, 0, 4 \n"

//   " LBCO &r14, C31, 0, 4 \n"//third operand is the offset
	" LBCO &r14, C31, r15, 4 \n"//third operand is the offset
   " JMP R3.w2 \n"
  );

  /* unreached */
  return 0;
}
/*
void pwm(void)
{
	__asm__ __volatile__
	(
    //EPWM_TBCTL
    " LDI32 r8, 0x48300000 \n"
	" LDI32 r9, 0x00000503 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_CMPB
    " LDI32 r8, 0x48300014 \n"
	" LDI32 r9, 0x00001388 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_CMPA
    " LDI32 r8, 0x48300012 \n"
	" LDI32 r9, 0x00001388 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_TBPRD
    " LDI32 r8, 0x4830000A \n"
	" LDI32 r9, 0x0000C350 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_TBCNT
    " LDI32 r8, 0x48300008 \n"
	" LDI32 r9, 0x00000000 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_AQCTLA
    " LDI32 r8, 0x48300016 \n"
	" LDI32 r9, 0x00000032 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_AQCTLB
    " LDI32 r8, 0x48300018 \n"
	" LDI32 r9, 0x00000302 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_TBCNT
    " LDI32 r8, 0x48300008 \n"
	" LDI32 r9, 0x00000000 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
	
	__asm__ __volatile__
	(
    //EPWM_TBCTL
    " LDI32 r8, 0x48300000 \n"
	" LDI32 r9, 0x00000500 \n" 
    " SBBO &r9, r8, 0, 4 \n"
	);
}*/

uint32_t di(void)
{
	return __R31;
}

void delay(register uint32_t x)
{
  __asm__ __volatile__
  (
	" LDI32 r8, 1000 \n"
	//" SUB r8, r14, 0 \n"
	
	"WAIT: \n"
	" SUB r8, r8, 1 \n"
	" QBNE WAIT, r8, 0 \n"
  );

}