// Control the backlight LED blinking
void rgb_lcd::blinkLED(void)
{
    // blink period in seconds = (<reg 7> + 1) / 24
    // on/off ratio = <reg 6> / 256
    setReg(0x07, 0x17);  // blink every second
    setReg(0x06, 0x7f);  // half on, half off
}

int main(int argc, char ** argv)
{
    int fd = openThrusters("/dev/motor");

//    int fd = open("/dev/ttyUSB0", O_RDWR);
//    printf("\nSetting address: %d\n", writeReg(fd, 1, REG_ADDR, 4));

    int i=0;

//    int err=0;
//    int e1=0, e2=0, e3=0, e4=0;
    setReg(fd, 1, REG_TIMER, 0);
    setReg(fd, 2, REG_TIMER, 0);
    setReg(fd, 3, REG_TIMER, 0);
    setReg(fd, 4, REG_TIMER, 0);


    for(i=0; i<10; i++)
    {
        printf("\n");
        printf("\nResult 1 is: %d\n", setSpeed(fd, 1, 0));
        printf("\nResult 2 is: %d\n", setSpeed(fd, 2, 0));
        printf("\nResult 3 is: %d\n", setSpeed(fd, 3, 0));
        printf("\nResult 4 is: %d\n", setSpeed(fd, 4, 0));
    }

    fsync(fd);
    printf("\n");
    return 0;
}

/*
** ===================================================================
**     Method      :  SetCV (component FreeCntr8)
**
**     Description :
**         Sets compare or preload register value. The method is called 
**         automatically as a part of several internal methods.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void SetCV(word Val)
{
  EnterCritical();                     /* Disable global interrupts */
  setReg(TMR0_CMPLD1,Val);             /* Store given value to the compare preload 1 register */
  setReg(TMR0_CMPLD2,Val);             /* Store given value to the compare preload 2 register */
  ExitCritical();                      /* Enable global interrupts */
}

/*ARD US*/ void rgb_lcd::begin(uint8_t cols, uint8_t lines, uint8_t dotsize){

    Wire.begin();
    
    if (lines > 1) {
        _displayfunction |= LCD_2LINE;
    }
    _numlines = lines;
    _currline = 0;

    // for some 1 line displays you can select a 10 pixel high font
    if ((dotsize != 0) && (lines == 1)) {
        _displayfunction |= LCD_5x10DOTS;
    }

    // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION!
    // according to datasheet, we need at least 40ms after power rises above 2.7V
    // before sending commands. Arduino can turn on way befer 4.5V so we'll wait 50
    delayMicroseconds(50000);


    // this is according to the hitachi HD44780 datasheet
    // page 45 figure 23

    // Send function set command sequence
    command(LCD_FUNCTIONSET | _displayfunction);
    delayMicroseconds(4500);  // wait more than 4.1ms

    // second try
    command(LCD_FUNCTIONSET | _displayfunction);
    delayMicroseconds(150);

    // third go
    command(LCD_FUNCTIONSET | _displayfunction);


    // finally, set # lines, font size, etc.
    command(LCD_FUNCTIONSET | _displayfunction);

    // turn the display on with no cursor or blinking default
    _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;
    display();

    // clear it off
    clear();

    // Initialize to default text direction (for romance languages)
    _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT;
    // set the entry mode
    command(LCD_ENTRYMODESET | _displaymode);
    
    
    // backlight init
    setReg(0, 0);
    setReg(1, 0);
    setReg(0x08, 0xAA);     // all led control by pwm
    
    setColorWhite();

}

/*
** ===================================================================
**     Method      :  PWMC1_SetRatio16 (component PWMMC)
**
**     Description :
**         This method sets a new duty-cycle ratio for selected channel.
**         The value is loaded after calling <Load> method.
**         
**         Version specific information for Freescale 56800/E and HC08
**         and HCS12 and HCS12X derivatives ] 
**         Setting is valid for actual speed mode only, initial value
**         is restored after speed mode change.
**         
**         Version specific information for Freescale 56800/E
**         derivatives - eFlexPWM device ] 
**         Settings is not affected during speed mode change. It should
**         be handled by the user code.
**     Parameters  :
**         NAME            - DESCRIPTION
**         Channel         - channel number (0 - 5). The number
**                           corresponds to the logical channel number
**                           assigned in the component settings, which
**                           may not correspond to channel number of the
**                           peripheral.
**         Ratio           - Ratio is expressed as an 16-bit
**                           unsigned integer number. 0 - 65535 value is
**                           proportional to ratio 0 - 100%.
**                           
**                           Version specific information for Freescale
**                           56800/E derivatives - eFlexPWM device ] 
**                           - _edge-aligned mode:_ value computed from
**                           parameter is written into PWM clear-edge
**                           (eFlexPWM_SMn_FRACVALx) register; PWM
**                           set-edge (eFlexPWM_SMn_FRACVALx) register
**                           is not affected (zero value assumed); where
**                           x = 2, 4. 
**                           - _center-aligned mode:_ value computed
**                           from parameter value is split between PWM
**                           set-edge (eFlexPWM_SMn_FRACVALx) and PWM
**                           clear-edge (eFlexPWM_SMn_FRACVAL(x+1))
**                           registers; where x = 2, 4.
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_NOTAVAIL - Channel is disabled
**                           ERR_RANGE - Parameter Channel is out of
**                           range
** ===================================================================
*/
byte PWMC1_SetRatio16(byte Channel,word Ratio)
{
  register word dutyreg;

  dutyreg = (word)((dword)getReg(PWM_PWMCM)*Ratio/65535); /* Calculate real duty */
  switch (Channel) {
  case 0 :
      setReg(PWM_PWMVAL0,dutyreg);     /* Store ratio value to the duty-compare register 0 */
      break;
  case 1 :
      setReg(PWM_PWMVAL1,dutyreg);     /* Store ratio value to the duty-compare register 1 */
      break;
  case 2 :
      setReg(PWM_PWMVAL2,dutyreg);     /* Store ratio value to the duty-compare register 2 */
      break;
  case 3 :
      setReg(PWM_PWMVAL3,dutyreg);     /* Store ratio value to the duty-compare register 3 */
      break;
  case 4 :
      return ERR_NOTAVAIL;
  case 5 :
      return ERR_NOTAVAIL;
  default: return ERR_RANGE;
  }
  return ERR_OK;
}

void main(void)
{
  /* Write your local variable definition here */
  int a = 0;
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
	PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  /* Write your code here */

	setReg(ADC_CLIST1,0x12);
	setReg(ADC_SDIS,0xFF88);
	setRegBitGroup(ADC_CTRL1,SMODE,0x01);
	AD1_Enable();
	

	Cpu_EnableInt();

	while(1)
	{
		WDog1_Clear();
		a++;
		if(a)
			a = 0;

	}
}

/*
** ===================================================================
**     Method      :  AS1_Init (component AsynchroSerial)
**
**     Description :
**         Initializes the associated peripheral(s) and the bean internal 
**         variables. The method is called automatically as a part of the 
**         application initialization code.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void AS1_Init(void)
{
  SerFlag = 0U;                        /* Reset flags */
  /* SCI_SCICR: LOOP=0,SWAI=0,RSRC=0,M=0,WAKE=0,POL=0,PE=0,PT=0,TEIE=0,TIIE=0,RFIE=0,REIE=0,TE=0,RE=0,RWU=0,SBK=0 */
  setReg(SCI_SCICR, 0U);               /* Set the SCI configuration */
  /* SCI_SCIBR: ??=0,??=0,??=0,SBR=17 */
  setReg(SCI_SCIBR, 17U);              /* Set prescaler bits */
  HWEnDi();                            /* Enable/disable device according to status flags */
}

/**
 * sets output.
 * @param OutCTL, if TRUE sets the output channels pad (on/off).
 * @param Outputs is the channel structure (see above).
 * @return ERR_OK always.
 */
byte PWMC1_setOutput(bool OutCTL, TChannels Outputs)
{
	if (OutCTL)
		setReg (PWMB_PMOUT, ((*(byte*)&Outputs) & 63) | 48896);
	else
		setReg (PWMB_PMOUT, 32768);
	
	return ERR_OK;
}

int _cdecl intrpt()
{
	USHORT itr = 0;
	int cycle = 0;
	(*getReg)(REG_A, &itr);
	switch (itr)
	{
		case 0:
			(*setReg)(REG_B, hddState);
			(*setReg)(REG_C, hddError);
		case 1:
			(*getReg)(REG_X, &throwItr);
		case 2:
			(*setReg)(REG_B, hdInfo.size);
		case 3:
		{
			switch (hddState)
			{
				case HDD_STATE_READY:
					(getReg)(REG_X, &itr);
					thrArg = (thrArg << 16) + itr;
					(getReg)(REG_Y, &itr);
					thrArg = (thrArg << 16) + itr;
					(getReg)(REG_Z, &itr);
					thrArg = (thrArg << 16) + itr;
					threadH = CreateThread(NULL, 0, &HDDThreadRead, (LPVOID)(&thrArg), 0, &threadID);
					setReg(REG_B, 1);
					break;
				case HDD_STATE_BUSY:
					setError(HDD_ERROR_BUSY);
					setReg(REG_B, 0);
					break;
			}
		}
		case 4:
		{
			switch (hddState)
			{
				case HDD_STATE_READY:
					(getReg)(REG_X, &itr);
					thrArg = (thrArg << 16) + itr;
					(getReg)(REG_Y, &itr);
					thrArg = (thrArg << 16) + itr;
					(getReg)(REG_Z, &itr);
					thrArg = (thrArg << 16) + itr;
					threadH = CreateThread(NULL, 0, &HDDThreadWrite, (LPVOID)(&thrArg), 0, &threadID);
					setReg(REG_B, 1);
					break;
				case HDD_STATE_BUSY:
					setError(HDD_ERROR_BUSY);
					setReg(REG_B, 0);
					break;
			}
		}
	}
	return cycle;
}

/*
** ===================================================================
**     Method      :  Inhr6_Init (component AsynchroSerial)
**
**     Description :
**         Initializes the associated peripheral(s) and the bean internal 
**         variables. The method is called automatically as a part of the 
**         application initialization code.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void Inhr6_Init(void)
{
  SerFlag = 0;                         /* Reset flags */
  /* SCI_CTRL1: LOOP=0,SWAI=0,RSRC=0,M=0,WAKE=0,POL=0,PE=0,PT=0,TEIE=0,TIIE=0,RFIE=0,REIE=0,TE=0,RE=0,RWU=0,SBK=0 */
  setReg(SCI_CTRL1, 0);                /* Set the SCI configuration */
  /* SCI_RATE: SBR=26,FRAC_SBR=0 */
  setReg(SCI_RATE, 208);               /* Set prescaler bits */
  HWEnDi();                            /* Enable/disable device according to status flags */
}

/*
** ===================================================================
**     Method      :  COUNTER2_Init (component EventCntr16)
**
**     Description :
**         Initializes the associated peripheral(s) and the beans 
**         internal variables. The method is called automatically as a 
**         part of the application initialization code.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void COUNTER2_Init(void)
{
  /* TMR1_CTRL: CM=0,PCS=0,SCS=0,ONCE=0,LENGTH=0,DIR=0,Co_INIT=0,OM=0 */
  setReg(TMR1_CTRL,0);                 /* Stop all functions of the timer */
  /* TMR1_SCR: TCF=0,TCFIE=0,TOF=0,TOFIE=1,IEF=0,IEFIE=0,IPS=0,INPUT=0,Capture_Mode=0,MSTR=0,EEOF=0,VAL=0,FORCE=0,OPS=0,OEN=0 */
  setReg(TMR1_SCR,4096);               /* Enable interrupt on overflow, select input edge polarity */
  /* TMR1_COMSCR: DBG_EN=0,??=0,??=0,??=0,??=0,??=0,??=0,TCF2EN=0,TCF1EN=0,TCF2=0,TCF1=0,CL2=0,CL1=0 */
  setReg(TMR1_COMSCR,0);               /* Disable interrupt on compare, disable compare preload */
  setReg(TMR1_CNTR,0);                 /* Initialize counter register */
  /* TMR1_CTRL: CM=1,PCS=2,SCS=0,ONCE=0,LENGTH=0,DIR=0,Co_INIT=0,OM=0 */
  setReg(TMR1_CTRL,9216);              /* Initialize timer control register */
}

/*
** ===================================================================
**     Method      :  Resolver_Init (component SynchroMaster)
**
**     Description :
**         Initializes the associated peripheral(s) and the beans 
**         internal variables. The method is called automatically as a 
**         part of the application initialization code.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void Resolver_Init(void)
{
  /* QSPI0_SCTRL: SPR=4,DSO=0,ERRIE=0,MODFEN=0,SPRIE=0,SPMSTR=1,CPOL=1,CPHA=1,SPE=0,SPTIE=0,SPRF=0,OVRF=0,MODF=1,SPTE=0 */
  setReg(QSPI0_SCTRL,0x81C2);          /* Set control register */
  /* QSPI0_DSCTRL: WOM=0,??=0,??=0,BD2X=0,SSB_IN=0,SSB_DATA=1,SSB_ODM=0,SSB_AUTO=0,SSB_DDR=1,SSB_STRB=0,SSB_OVER=0,SPR3=0,DS=0x0B */
  setReg(QSPI0_DSCTRL,0x048B);         /* Set data size and control register */
  /* QSPI0_DELAY: ??=0,??=0,??=0,WAIT=1 */
  setReg(QSPI0_DELAY,0x01);            /* Set data size and control register */
  SerFlag = 0;                         /* Reset all flags */
  EnUser = FALSE;                      /* Disable device */
  HWEnDi();                            /* Enable/disable device according to the status flags */
}

void memoryAcessControl(uint64_t ir){
    if ((ir != 10)&&(ir>=1)&&(ir<=12)){
        turnON(READMEM_FLAG);
    }else if (ir == 33){
        turnON(WRITEMEM_FLAG);
        setReg(MSK, 0);
    }else if (ir == 18){
        turnON(WRITEMEM_FLAG);
        setReg(MSK, LCLEAN);
    }else if (ir == 19){
        turnON(WRITEMEM_FLAG);
        setReg(MSK, RCLEAN);
    }
}

/*
** ===================================================================
**     Method      :  Timer_Init 
**
**     Description :
**         This method is internal. It is used by Processor Expert
**         only.
** ===================================================================
*/
void Timer_Init(void)
{
  /* TMRC0_CTRL: CM=0,PCS=0,SCS=0,ONCE=0,LENGTH=1,DIR=0,Co_INIT=0,OM=0 */
  setReg(TMRC0_CTRL,32);               /* Stop all functions of the timer */
  /* TMRC1_CTRL: CM=7,PCS=4,SCS=0,ONCE=0,LENGTH=1,DIR=0,Co_INIT=0,OM=0 */
  setReg(TMRC1_CTRL,59424);            /* Set up cascade counter mode */
  setReg(TMRC1_CNTR,0);                /* Reset counter register */
  setReg(TMRC0_CNTR,0);
  setReg(TMRC1_LOAD,0);                /* Reset load register */
  setReg(TMRC0_LOAD,0);
  setReg(TMRC1_CMP1,639);             /* Store given value to the compare registers */
  setReg(TMRC0_CMP1,62499);
  setRegBitGroup(TMRC0_CTRL,PCS,8);  /* Store given value to the prescaler */
  setReg(TMRC0_CNTR,0);                /* Reset counter */
  setReg(TMRC1_CNTR,0);
}

ret_t rtl8370_setAsicRegBits(uint32 reg, uint32 bits, uint32 value)
{
    uint32 regData;    
    uint32 bitsShift;    
    uint32 valueShifted;        

    if(reg > RTL8370_REGDATAMAX )
	    return RT_ERR_INPUT;

    if(bits >= (1<<RTL8370_REGBITLENGTH) )
        return RT_ERR_INPUT;    

    bitsShift = 0;
    while(!(bits & (1 << bitsShift)))
    {
        bitsShift++;
        if(bitsShift >= RTL8370_REGBITLENGTH)
            return RT_ERR_INPUT;
    }

    valueShifted = value << bitsShift;
    if(valueShifted > RTL8370_REGDATAMAX)
        return RT_ERR_INPUT;

    regData = getReg(reg);
    regData = regData & (~bits);
    regData = regData | (valueShifted & bits);	    

	setReg(reg, regData);

    return RT_ERR_OK;
}

/**
 * initializes the counter/timer. the timer is initialized w/ 1ms period.
 */
void TI1_init (void)
{
	/* TMRA3_CTRL: CM=0,PCS=0,SCS=0,ONCE=0,LENGTH=1,DIR=0,Co_INIT=0,OM=0 */
	setReg (TMRA3_CTRL, 0x20);           /* Stop all functions of the timer */

	/* TMRA3_SCR: TCF=0,TCFIE=1,TOF=0,TOFIE=0,IEF=0,IEFIE=0,IPS=0,INPUT=0,Capture_Mode=0,MSTR=0,EEOF=0,VAL=0,FORCE=0,OPS=0,OEN=0 */
	setReg (TMRA3_SCR, 0x4000);
	setReg (TMRA3_LOAD, 0);                /* Reset load register */
	setReg (TMRA3_CMP1, 39999);            /* Store appropriate value to the compare register according to the selected high speed CPU mode */

	clrRegBits (TMRA3_CTRL, 0x1e00);
	setRegBits (TMRA3_CTRL, 4096);    /* Set prescaler register according to the selected high speed CPU mode */
	setReg 	   (TMRA3_CNTR, 0); 		   /* Reset counter */
		
	clrRegBits (TMRA3_CTRL, 0xe000);
	setRegBits (TMRA3_CTRL, 0x2000);	   /* counter on! */
}

/*
** ===================================================================
**     Method      :  AS1_SendChar (component AsynchroSerial)
**     Description :
**         Sends one character to the channel. If the component is
**         temporarily disabled (Disable method) SendChar method only
**         stores data into an output buffer. In case of a zero output
**         buffer size, only one character can be stored. Enabling the
**         component (Enable method) starts the transmission of the
**         stored data. This method is available only if the
**         transmitter property is enabled.
**         Version specific information for Freescale 56800 derivatives
**         DMA mode:
**         If DMA controller is available on the selected CPU and the
**         transmitter is configured to use DMA controller then this
**         method only sets selected DMA channel. Then the status of
**         the DMA transfer can be checked using GetCharsInTxBuf method.
**         See an example of a typical usage for details about
**         communication using DMA.
**     Parameters  :
**         NAME            - DESCRIPTION
**         Chr             - Character to send
**     Returns     :
**         ---             - Error code, possible codes:
**                           ERR_OK - OK
**                           ERR_SPEED - This device does not work in
**                           the active speed mode
**                           ERR_TXFULL - Transmitter is full
** ===================================================================
*/
byte AS1_SendChar(AS1_TComData Chr)
{
  if ((getRegBit(SCI_SCISR, TDRE) == 0U)) { /* Is the transmitter empty? */
    return ERR_TXFULL;                 /* If yes then error */
  }
  setReg(SCI_SCIDR, Chr);              /* Store char to transmitter register */
  return ERR_OK;                       /* OK */
}

/*
** ===================================================================
**     Method      :  PIT_1ms_Init (component TimerInt)
**
**     Description :
**         Initializes the associated peripheral(s) and the beans 
**         internal variables. The method is called automatically as a 
**         part of the application initialization code.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void PIT_1ms_Init(void)
{
  /* PIT0_CTRL: SLAVE=0,??=0,??=0,??=0,??=0,??=0,CLKSEL=0,??=0,PRESCALER=0,PRF=0,PRIE=1,CNT_EN=0 */
  setReg(PIT0_CTRL,0x02);              /* Set up control register */
  SetCV((word)0xC350);                 /* Store appropriate value to the compare register according to the selected high speed CPU mode */
  SetPV((byte)0x01);                   /* Set prescaler register according to the selected high speed CPU mode */
  HWEnDi();                            /* Enable/disable device according to status flags */
}

/*
** ===================================================================
**     Method      :  Bits1_PutVal (component BitsIO)
**
**     Description :
**         This method writes the new output value.
**           a) direction = Input  : sets the new output value;
**                                   this operation will be shown on
**                                   output after the direction has
**                                   been switched to output
**                                   (SetDir(TRUE);)
**           b) direction = Output : directly writes the value to the
**                                   appropriate pins
**     Parameters  :
**         NAME       - DESCRIPTION
**         Val        - Output value (0 to 3)
**     Returns     : Nothing
** ===================================================================
*/
void Bits1_PutVal(byte Val)
{
  register word Temp;                  /* Temporary variable */

  Temp = (((word)Val)<<6) & Bits1_PIN_MASK; /* Prepare value for output */
  Shadow_GPIO_B_DATA = Shadow_GPIO_B_DATA & ~Bits1_PIN_MASK | Temp; /* Set-up bits in shadow variable */
  setReg(GPIO_B_DATA,(getReg(GPIO_B_DATA)) & ~Bits1_PIN_MASK | Temp); /* Set-up bits on port */
}

/**
 * Enables the PWM pad and clears fault pins.
 * @return ERR_OK always.
 */
byte PWMC1_outputPadEnable (void)
{
	setRegBit (PWMB_PMOUT, PAD_EN);
#ifndef EMERGENCY_DISABLED
	setReg (PWMB_PMFSA, 0x55);
#endif	
	return ERR_OK;
}