#include "fsm.h"

#include <why-irq.h>

#include <cassert>
#include <cstring>


FSM::FSM(
    Why::TimeSpec interval,
    int sensor_burning_irq, Switch& hard_alarm,
    long soft_critical_temp, Switch& soft_alarm,
    int operator_button_irq,
    const std::string& can_iface,
    int can_id,
    Sensor& sensor, Switch& cooling,
    long low_temp, long high_temp)
: _interval(interval),
  _interval_timer([this](){_timer_tick();}),
  _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), 
  _can_controller(can_iface),
  _can_id(can_id),
  _state(State::INIT)
{}

void FSM::start()
{
    _lock.lock();
    switch (_state) {
        case State::INIT:
            _interval_timer.start(Why::TimeSpec(0,1), _interval);
            Why::IRQ::connect(_sensor_burning_irq, [this](int irqnum){_sensor_burning();});
            Why::IRQ::connect(_operator_button_irq, [this](int irqnum){_operator_button_pressed();});
            Why::IRQ::connect(CAN_JF0815_HW::IRQ_XMIT_DONE, [this](int irqnum){_can_xmit_done();});
            _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)                   // <-- xmit/enqueue notifications
{                                                      //     change state
    uint32_t notify_state;
    switch (state) {
        case State::INIT:
            notify_state = 0;
            break;
        case State::RUNNING:
            notify_state = 1;
            break;
        case State::OVERTEMP:
            notify_state = 2;
            break;
        case State::SENSORBURNING:
            notify_state = 3;
            break;
    }

    CANFrame frame;
    frame.id = _can_id;
    frame.dlc = sizeof(notify_state);
    memcpy(frame.data, &notify_state, sizeof(notify_state));
    
    if (_can_controller.xmit_busy())
        _pending_notifications.push_back(frame);
    else
        _can_controller.begin_xmit(frame);

    _state = state;
}

void FSM::_can_xmit_done()                             // <-- send pending frame if any
{
    _lock.lock();
    if (_pending_notifications.size() > 0) {
        CANFrame frame = _pending_notifications.front();
        _pending_notifications.pop_front();
        _can_controller.begin_xmit(frame);
    }
    _lock.unlock();
}