#include "fsm.h"

#include <why-irq.h>

#include <cassert>
#include <cstring>


FSM::FSM(
    int sensor_burning_irq, Switch& hard_alarm,
    long soft_critical_temp, Switch& soft_alarm,
    int operator_button_irq,
    Why::MessageQueue<State>& state_change_queue,
    Sensor& sensor, Switch& cooling,
    long low_temp, long high_temp)
: _sensor_burning_irq(sensor_burning_irq),
  _hard_alarm(hard_alarm),
  _soft_critical_temp(soft_critical_temp),
  _soft_alarm(soft_alarm),
  _operator_button_irq(operator_button_irq),
  _sensor(sensor), 
  _cooling(cooling), 
  _low_temp(low_temp), 
  _high_temp(high_temp), 
  _state_change_queue(state_change_queue),
  _state(State::INIT)
{}

void FSM::start()
{
    _lock.lock();
    switch (_state) {
        case State::INIT:
            Why::IRQ::connect(_sensor_burning_irq, [this](int irqnum){_sensor_burning();});
            Why::IRQ::connect(_operator_button_irq, [this](int irqnum){_operator_button_pressed();});
            _sensor.turn_on();
            _state_change(State::RUNNING);
            break;
        case State::RUNNING:
            assert(!"RUNNING");
            break;
        case State::OVERTEMP:
            assert(!"OVERTEMP");
            break;
        case State::SENSORBURNING:
            assert(!"SENSORBURNING");
            break;
    }
    _lock.unlock();
}

void FSM::timer_tick()
{
    _lock.lock();
    switch (_state) {
        case State::INIT:
            assert(!"INIT");
            break;
        case State::RUNNING: {
            double temp = _sensor.get_temperature();
            
            if (temp > _soft_critical_temp) {
                _cooling.set_state(true);
                _soft_alarm.set_state(true);
                _state_change(State::OVERTEMP);
            }
            else if (temp > _high_temp)
                _cooling.set_state(true);
            else if (temp < _low_temp)
                _cooling.set_state(false);
            break;
        }
        case State::OVERTEMP: {
            double temp = _sensor.get_temperature();
            
            if (temp > _high_temp)
                _cooling.set_state(true);
            else if (temp < _low_temp)
                _cooling.set_state(false);
            break;
        }
        case State::SENSORBURNING:
            break;
    }
    _lock.unlock();
}
    
void FSM::_sensor_burning()
{
    _lock.lock();
    switch (_state) {
        case State::INIT:
            assert(!"INIT");
            break;
        case State::RUNNING:
            _sensor.turn_off();
            _cooling.set_state(false);
            _hard_alarm.set_state(true);
            _state_change(State::SENSORBURNING);
            break;
        case State::OVERTEMP:
            _sensor.turn_off();
            _cooling.set_state(false);
            _soft_alarm.set_state(false);
            _hard_alarm.set_state(true);
            _state_change(State::SENSORBURNING);
            break;
        case State::SENSORBURNING:
            break;
    }
    _lock.unlock();
}

void FSM::_operator_button_pressed()
{
    _lock.lock();
    switch (_state) {
        case State::INIT:
            assert(!"INIT");
            break;
        case State::RUNNING:
            break;
        case State::OVERTEMP:
            _soft_alarm.set_state(false);
            _state_change(State::RUNNING);
            break;
        case State::SENSORBURNING:
            break;
    }
    _lock.unlock();
}

void FSM::_state_change(State state)
{
    auto written = 
        _state_change_queue.put_noblock(state);
    if (!written)
        std::println("state changes too fast?!");
    _state = state;
}