#include "ThermostatInit.h"

/***************************************************************************/
/*                     System Init Functions                               */
/***************************************************************************/
void vfnClock_Init(void)
{

  /* ICSC2: BDIV=0:1 (Divide by 2) FLL Freq = (XTAL * 512)/BDIV = 8.388MHz (Bus ~4.194MHz)
     HGO = 0, LP = 1 FLL Disabled in Stop modes, High Gain Off
     EREFS = 1, ERCLKEN = 1, EREFSTEN = 1, Extenal Reference Enabled in all modes
  */
  ICSC2 = 0x47;
  
  /* Wait until the initialization of the external crystal oscillator is completed */
  while(!ICSSC_OSCINIT)
  {
    __RESET_WATCHDOG();
  }
  
  /*  ICSC1: 
      CLKS=0 Clock Source is Output of the FLL
      RDIV=0 Reference Divider = 1, 32.768 External Crystal
      IREFS=0,IRCLKEN=0,IREFSTEN=0 Internal Reference Disabled in All Modes
  */
  ICSC1 = 0x00;
  
  /*  ICSSC
      DRST = 0:0  Low Range
      DMX32 = 0 Do not *set the max frequency from the 32KHz XTAL
  */
  ICSSC &= (unsigned char)~0xE0;
  /* Wait until the DCO locks  in a low-range mode */
  while((ICSSC & 0xC0) != 0x00)
  {
    __RESET_WATCHDOG();
  }
}

UINT8 u8fnSwitchToFBELP (void)  
{
  UINT16 OSC_Delay = 0;
  ICSC1 = ICSC1_CLKS1_MASK; 
  ICSC2_LP = 1;
  while ((ICSSC & 0x0C ) != 0x08)
  {
    if (!--OSC_Delay)
    {
      return (OSC_ERROR);
    }
  }
  return (OSC_OK);
  
}

void vfnSystem_Init(void)
{
  #ifdef DEBUG_ON
    /* SOPT1: Enable COP and Stop Instructions. BKGD and RST Pins remain enabled for debugging purposes */
    SOPT1 = SOPT1_STOPE_MASK | SOPT1_COPT_MASK | SOPT1_BKGDPE_MASK | SOPT1_RSTPE_MASK;
  #else  
    /* SOPT1: Enable COP and Stop Instructions. RST Pin enabled */
    SOPT1 = SOPT1_COPE_MASK | SOPT1_COPT_MASK | SOPT1_STOPE_MASK | SOPT1_RSTPE_MASK;
  #endif
  /* SOPT2 Select Bus Clock as the source for COP*/
  SOPT2 = SOPT2_COPCLKS_MASK;
  /* SPMSC1 Enable Reset with LV conditions. LVD enabled in Run and Stop Modes. Enable Bandgap Channel */
  //SPMSC1 = SPMSC1_LVDACK | SPMSC1_LVDRE_MASK | SPMSC1_LVDE_MASK | SPMSC1_BGBE_MASK;
  SPMSC1 = SPMSC1_LVDACK | SPMSC1_LVDRE_MASK | SPMSC1_BGBE_MASK;
  /* SPMSC2 Enable Partial Power Down Modes. Stop 3 Enabled */
  SPMSC2 = SPMSC2_LPR_MASK | SPMSC2_LPWUI_MASK | SPMSC2_PPDACK_MASK | SPMSC2_PPDE_MASK;
  /* SCGC1: Enable TPM1 & 2 and ADC. Disable IIC and SCI */
  SCGC1 = SCGC1_TPM2_MASK | SCGC1_TPM1_MASK | SCGC1_ADC_MASK;
  #ifdef DEBUG_ON
    /* SCGC2: Enable Debug, Flash, IRQ, KBI, ACMP, TOD and LCD. Disable SPI */
    SCGC2 = SCGC2_DBG_MASK | SCGC2_FLS_MASK | SCGC2_IRQ_MASK | SCGC2_KBI_MASK | SCGC2_ACMP_MASK | SCGC2_TOD_MASK | SCGC2_LCD_MASK;
  #else
    /* SCGC2: Enable IRQ, KBI, ACMP, TOD and LCD. Disable SPI */
    SCGC2 = SCGC2_FLS_MASK | SCGC2_IRQ_MASK | SCGC2_KBI_MASK | SCGC2_ACMP_MASK | SCGC2_TOD_MASK | SCGC2_LCD_MASK;
  #endif
}


void vfnGoToLowPower(void)
{
  /* SCGC1: Disable TPM1 & 2 and ADC, IIC and SCI. Non of these work on Stop mode */
  SCGC1 = 0x00;
  /* SCGC2: LCD, IRQ and KBI remain Enable on Stop. Disable Debug, SPI & ACMP*/
  SCGC2 = SCGC2_IRQ_MASK | SCGC2_KBI_MASK | SCGC2_TOD_MASK | SCGC2_LCD_MASK;
  
  //SPMSC1 = SPMSC1_LVDACK | SPMSC1_LVDRE_MASK | SPMSC1_LVDE_MASK;
  SPMSC1 = SPMSC1_LVDACK | SPMSC1_LVDRE_MASK;
  
  //(void)u8fnSwitchToFBELP();
  //_Stop;
}


void vfnExitLowPower(void)
{
  //vfnClock_Init();
  /* SCGC1: Enable TPM1 & 2 and ADC. Disable IIC and SCI */
  SCGC1 = SCGC1_TPM2_MASK | SCGC1_TPM1_MASK | SCGC1_ADC_MASK;
  #ifdef DEBUG_ON
    /* SCGC2: Enable Debug, IRQ, KBI, ACMP and LCD. Disable SPI */
    SCGC2 = SCGC2_DBG_MASK | SCGC2_FLS_MASK | SCGC2_IRQ_MASK | SCGC2_KBI_MASK | SCGC2_ACMP_MASK | SCGC2_TOD_MASK | SCGC2_LCD_MASK;
  #else
    /* SCGC2: Enable IRQ, KBI, ACMP and LCD. Disable SPI */
    SCGC2 = SCGC2_FLS_MASK | SCGC2_IRQ_MASK | SCGC2_KBI_MASK | SCGC2_ACMP_MASK | SCGC2_TOD_MASK | SCGC2_LCD_MASK;
  #endif  
  
  SPMSC1 = SPMSC1_LVDACK | SPMSC1_LVDRE_MASK | SPMSC1_LVDE_MASK | SPMSC1_BGBE_MASK;  
}

