#include <ch.h>
#include <hal.h>
#include "sensor_task.h"
#include "stm32l_rtc.h"
#include "main_logic.h"
#include "mma7660_acc.h"
#include "stats.h"
#include "parameters.h"

/* Return percentage 0-100% of the likelihood of being outside at this time. */
static int schedule_estimate(const time_t *time)
{
    // Schedule by the hour of the day.
    static const int schedule_timeofday[24] = {
        //0    1    2    3    4    5    6    7    8    9   10   11
         25,   0,   0,   0,   0,   0,   0, 100, 100,  50,  50,  50,
        
        //12  13   14   15   16   17   18   19   20   21   22   23
         50,  50,  75,  75,  75,  75,  75,  75, 100, 100,  50,  50
    };
    
    int p1 = schedule_timeofday[time->hour];
    int p2 = schedule_timeofday[(time->hour + 1) % 24];
    int sched1 = p1 + (p2 - p1) * time->minute / 60;
    
    static const int schedule_month[12] = {
      //Jan     Feb     Mar    Apr     May      Jun
        100,    100,    100,    75,     50,      25,
    
      //Jul     Aug     Sep    Oct     Nov      Dec
         25,     50,     75,   100,    100,     100
    };
    
    p1 = schedule_month[time->month - 1];
    p2 = schedule_month[time->month % 12];
    int sched2 = p1 + (p2 - p1) * time->day / 30;
    
    return sched1 * sched2 / 100;
}

static void reset_watchdog()
{
    IWDG->KR = 0xAAAA;
}



static system_state_t g_state = STATE_POLL;
static int g_sleep_timer;
static int g_prevx, g_prevy, g_prevz;
static int g_stats_timer;

int main_logic_step()
{
    system_state_t new_state = g_state;
    int sleep_time = 0;
    
    time_t time;
    if (!get_rtc(&time))
    {
        // Dummy time
        time.day = 1;
        time.month = 12;
        time.year = 2012;
        time.hour = 18;
        time.minute = 0;
        time.second = 0;
    }
    
    bool should_sleep = (is_sun_up(&time) || (schedule_estimate(&time) == 0));
    
    switch (g_state)
    {
        case STATE_DEEPSLEEP:
            if (!should_sleep)
            {
                new_state = STATE_POLL;
            }
            else
            {
                sleep_time = 15000; // Have to wake up to keep IWDG happy.
            }
            
            break;
        
        case STATE_POLL:
        {
            int x, y, z;
            mma7660_poweron(MMA7660_SR_16);
            mma7660_wait();
            mma7660_read(&x, &y, &z);
            mma7660_poweroff();
            
            stats_add_power(200, 100); // 200µA when sensor is on
            
            int dx = g_prevx - x;
            int dy = g_prevy - y;
            int dz = g_prevz - z;
            g_prevx = x;
            g_prevy = y;
            g_prevz = z;
            
            int delta = dx * dx + dy * dy + dz * dz;
            
            if (delta > WAKEUP_ACCELERATION * WAKEUP_ACCELERATION)
            {
                new_state = STATE_ACTIVE;
            }
            else if (should_sleep)
            {
                new_state = STATE_DEEPSLEEP;
            }
            else if (g_stats_timer > 3600000)
            {
                // Save the statistics once an hour
                stats_add_power(1000, 5000); // EEPROM writing takes about 5 seconds and 1 mA
                stats_save();
                g_stats_timer = 0;
            }
            else
            {
                // Choose the poll interval based on the probability of movement
                // at this time of day.
                int probability = schedule_estimate(&time);
                sleep_time = POLL_INTERVAL_MAX - probability * (POLL_INTERVAL_MAX - POLL_INTERVAL_MIN) / 100;
            }
            
            break;
        }
        
        case STATE_ACTIVE:
        {
            int movement = sensors_movement();
            int temperature = sensors_temperature();
            int schedule = schedule_estimate(&time);
            
            int total = movement * temperature * schedule / 100 / 100;
            
            if (total > 0)
            {
                // Choose blink interval based on the total estimate.
                // 100 % => 500 ms, 0 % => 2000 ms
                int interval = 2000 - total * 1500 / 100;
                
                for (int i = 0; i < 3; i++)
                {
                    palClearPad(GPIOA, GPIOA_LEDS);
                    chThdSleepMilliseconds(10);
                    palSetPad(GPIOA, GPIOA_LEDS);
                    chThdSleepMilliseconds(40);
                }
                
                stats_add_blink();
                stats_add_power(40000, 30); // 40 mA to leds
                stats_add_power(350, interval + 150); // 350µA during wait
                stats_add_time(STATE_ACTIVE, interval + 150);
                
                chThdSleepMilliseconds(interval);
                
                g_sleep_timer = 0;
            }
            else
            {
                g_sleep_timer++;
                chThdSleepMilliseconds(1000);
                stats_add_power(350, 1000);
                stats_add_time(STATE_ACTIVE, 1000);
                
                if (g_sleep_timer > 30)
                {
                    // After 30 seconds of no blinks, go back to polling.
                    new_state = STATE_POLL;
                }
            }
            
            stats_add_histogram_entry();
        }
    }
    
    g_stats_timer += sleep_time;
    reset_watchdog();
    stats_add_time(g_state, sleep_time);
    
    /* State switch logic */
    if (new_state != g_state)
    {
        if (g_state == STATE_ACTIVE)
        {
            // Switching from active state, stop the sensor thread.
            sensors_stop();
        }
        
        if (new_state == STATE_ACTIVE)
        {
            // Switching to active state, start the sensor thread.
            sensors_start();
            g_sleep_timer = 0;
        }
        
        chSysLock();
        g_state = new_state;
        chSysUnlock();
    }
    
    return sleep_time;
}

system_state_t main_logic_state()
{
    return g_state;
}