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

volatile systime_t last_stay_on = 0;

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

bool powersave_check()
{
    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();
    
    chSysLock();
    SYSCFG->PMC = 0;
    
    palClearPad(GPIOB, GPIOB_RF_PWR);
    
    STM32_USB->CNTR = CNTR_PDWN;
    
    RCC->AHBLPENR = 0;
    RCC->APB2LPENR = 0;
    RCC->APB1LPENR = 0;
    
    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
    PWR->CR |= PWR_CR_CWUF;
    PWR->CR |= PWR_CR_PDDS;
    SCB->SCR |= SCB_SCR_SLEEPDEEP;
    PWR->CSR |= PWR_CSR_EWUP1;
    
    __sync_synchronize();
    
    asm("wfe");
}

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;
}