#include "powersave.h"
#include <ch.h>
#include <hal.h>
#include <stm32l1xx.h>
#include <stdio.h>
#include "sensor_task.h"
#include <stm32_usb.h>
#include "rf_task.h"

volatile systime_t last_stay_on = 0;
volatile bool sleep = false;

void powersave_stay_on()
{
    chSysLock();
    last_stay_on = chTimeNow();
    chSysUnlock();
}

void powersave_go_off()
{
    sleep = true;
}

bool powersave_check()
{
    if (sleep)
        return true;
    
    bool status;
    chSysLock();
    status = (chTimeNow() - last_stay_on) > MS2ST(SLEEP_TIME * 1000);
    chSysUnlock();
    return status;
}

void powersave_sleep()
{
    printf("System is going to sleep mode...\r\n");
    chThdSleepMilliseconds(100);
    
    sensor_stop();
    rf_stop();
    
    chSysLock();
    SYSCFG->PMC = 0;
    STM32_USB->CNTR = CNTR_PDWN;
    
    // Reset to get rid of IWDG and let main() put us to sleep
    RTC->BKP0R = 0x57400; // Magic code for sleep
    SCB->AIRCR = 0x05FA0000 | SCB_AIRCR_SYSRESETREQ;
}

static size_t writes(void *ip, const uint8_t *bp, size_t n) {
    PowersaveStream *stream = ip;
    return stream->real_stream->vmt->write(stream->real_stream, bp, n);
}

static size_t reads(void *ip, uint8_t *bp, size_t n) {
    PowersaveStream *stream = ip;
    size_t status = stream->real_stream->vmt->read(stream->real_stream, bp, n);
    
    if (status != 0)
        powersave_stay_on();
    
    return status;
}

bool_t putwouldblock(void *instance)
{
    PowersaveStream *stream = instance;
    return stream->real_stream->vmt->putwouldblock(stream->real_stream);
}

bool_t getwouldblock(void *instance)
{
    PowersaveStream *stream = instance;
    return stream->real_stream->vmt->getwouldblock(stream->real_stream);
}

msg_t put(void *instance, uint8_t b, systime_t time)
{
    PowersaveStream *stream = instance;
    return stream->real_stream->vmt->put(stream->real_stream, b, time);
}

msg_t get(void *instance, systime_t time)
{
    PowersaveStream *stream = instance;
    msg_t result = stream->real_stream->vmt->get(stream->real_stream, time);
    if (result >= 0) powersave_stay_on();
    return result;
}

size_t writet(void *instance, const uint8_t *bp, size_t n, systime_t time)
{
    PowersaveStream *stream = instance;
    return stream->real_stream->vmt->writet(stream->real_stream, bp, n, time);
}

size_t readt(void *instance, uint8_t *bp, size_t n, systime_t time)
{
    PowersaveStream *stream = instance;
    size_t result = stream->real_stream->vmt->readt(stream->real_stream, bp, n, time);
    if (result > 0) powersave_stay_on();
    return result;
}

static const struct BaseChannelVMT vmt = {writes, reads, putwouldblock, getwouldblock, put, get, writet, readt};

void PowersaveStreamInit(PowersaveStream *stream, BaseChannel *real_stream)
{
    stream->vmt = &vmt;
    stream->real_stream = real_stream;
}