/*
 *  Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com/ 
 *  
 *  Redistribution and use in source and binary forms, with or without 
 *  modification, are permitted provided that the following conditions 
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright 
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the 
 *    documentation and/or other materials provided with the   
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *  
 */
 
/*
 *  ======== PowerCC3200_nonos_policy.c ========
 */

#include <stdint.h>

/* driverlib header files */
#include <inc/hw_types.h>
#include <driverlib/prcm.h>
#include <driverlib/cpu.h>
#include <driverlib/rom.h>
#include <driverlib/rom_map.h>
#include <driverlib/utils.h>

#include <ti/drivers/Power.h>
#include <ti/drivers/power/PowerCC3200.h>
#include <ti/drivers/ports/HwiP.h>

/* bitmask of constraints that disallow LPDS */
#define LPDS_DISALLOWED (1 << PowerCC3200_DISALLOW_LPDS)

/* macro to pick two matching count values */
#define COUNT_WITHIN_TRESHOLD(a, b, c, th) \
        ((((b) - (a)) <= (th)) ? (b) : (c))


#define TRUE    1
#define FALSE   0

volatile uint32_t IdleTime = 0;

void PowerCC3200_sleepPolicy()
{
	unsigned long long RTCtimestamp = 0;
	unsigned long long idle_time_usec = IdleTime * 1000;
	unsigned long long delay_value= 0;
	unsigned long long delay_value_in_usec= 0;
	unsigned long long remain = 0;
	unsigned long long count[3];
	unsigned long constraintMask;
	unsigned long key;
	int i = 0;

    /*
     *  get the current RTC count, using the fast interface; to use the
     *  fast interface the count must be read three times, and then
     *  the value that matches on at least two of the reads is chosen
     */
    for (i = 0; i < 3; i++) {
        count[i] = PRCMSlowClkCtrFastGet();
    }
    RTCtimestamp = COUNT_WITHIN_TRESHOLD(count[0], count[1], count[2], 1);

	while(idle_time_usec > PowerCC3200_TOTALTIMELPDS)
	{	
		key = HwiP_disable();
		/* query the declared constraints */
    	constraintMask = Power_getConstraintMask();
	    /* check if we are allowed to go to LPDS */
		if ((constraintMask & LPDS_DISALLOWED) == 0)
		{
	    	/* check if can go to LPDS */
			remain = ((idle_time_usec - PowerCC3200_TOTALTIMELPDS) * 32768) / 1000000;

            /* if timer value is greater than the max value, set it equal to max
            value
            */
            if(remain >= 0xFFFFFFFF)
            {
                remain = 0xFFFFFFFF;
            }
            
			/* set the LPDS wakeup time interval */
			MAP_PRCMLPDSIntervalSet((unsigned long)remain);

			/* enable the wake source to be timer */
			MAP_PRCMLPDSWakeupSourceEnable(PRCM_LPDS_TIMER);

			/* go to LPDS mode */
			Power_sleep(PowerCC3200_LPDS);
			idle_time_usec = 0;

		}
		else
		{

			/* wait for interrupt pend*/
			MAP_PRCMSleepEnter();

		    for (i = 0; i < 3; i++) {
		        count[i] = PRCMSlowClkCtrFastGet();
		    }
		    delay_value = COUNT_WITHIN_TRESHOLD(count[0], count[1], count[2], 1);
			delay_value = delay_value - RTCtimestamp;
			RTCtimestamp += delay_value;
			delay_value_in_usec = ((delay_value *1000000)/32768);
			if(idle_time_usec > delay_value_in_usec)
			{
				idle_time_usec =-delay_value_in_usec;
			}
			else
			{
				idle_time_usec = 0;
			}
		}

		HwiP_restore(key);
	}
}

/*
 *  ======== PowerCC3200_initPolicy ========
 */
void PowerCC3200_initPolicy()
{
}
