summaryrefslogtreecommitdiff
path: root/pcr-controller/cmd_queue.c
blob: 65cf001e2260898cd663af288a2a60c8e7ca3898 (plain) (blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
/*
 *  r3PCR Teensy Controller Code
 *
 *
 *  Copyright (C) 2013 Bernhard Tittelbach <xro@realraum.at>
 *
 *  This code is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  any later version.
 *
 *  This code is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with spreadspace avr utils. If not, see <http://www.gnu.org/licenses/>.
*/

#include <stdlib.h>
#include "cmd_queue.h"


typedef struct {
    uint8_t read_num_numbers;
    void* function_ptr;
} cmd_queue_item;

#define CMQ_QUEUE_LENGTH 4
cmd_queue_item cmd_queue_[CMQ_QUEUE_LENGTH];
uint8_t cmd_queue_todo_pos_ = 0;
uint8_t cmd_queue_next_here_ = 0;

#define CMDQ_INPUT_LIST_LEN  2
int16_t input_list_[CMDQ_INPUT_LIST_LEN];
uint8_t num_args_read_ = 0;

#define CMDQ_READBUFFER_LEN  20
char cmdq_readbuffer_[CMDQ_READBUFFER_LEN];
char *cmdq_readbuffer_pos_ = cmdq_readbuffer_;
char *const cmdq_readbuffer_end_ = cmdq_readbuffer_ + (CMDQ_READBUFFER_LEN - 1);

inline void cmdq_finishReadingArgument(void)
{
    *cmdq_readbuffer_pos_ = '\0';
    cmdq_readbuffer_pos_ = cmdq_readbuffer_;
    input_list_[num_args_read_++] = atoi(cmdq_readbuffer_);
}

//call regularily if in state CQ_READING
uint8_t cmdq_addCharToArgumentBuffer(uint8_t c)
{
    if ( cmd_queue_todo_pos_ == cmd_queue_next_here_ )
        return 1; //nothing to do with us
    if (num_args_read_ >= CMDQ_INPUT_LIST_LEN)
        return 1;  //nothing to do with us
    if (num_args_read_ >= cmd_queue_[cmd_queue_todo_pos_].read_num_numbers )
        return 1;  //nothing to do with us

    //if input terminated by \n or \r then addArgument
    if (c == '\n' || c == '\r')
    {
        cmdq_finishReadingArgument();
        return 0;
    } else {    //continue reading
        *cmdq_readbuffer_pos_ = (char) c;
        cmdq_readbuffer_pos_++;
    }
    //if numlen of readbuffer reached, addArgument as well
    if (cmdq_readbuffer_pos_ == cmdq_readbuffer_end_)
    {
        cmdq_finishReadingArgument();
        return 0;
    }
    return 0;
}

void cmdq_queueCmdWithNumArgs(void* fptr, uint8_t num_args)
{
    if (num_args > CMDQ_INPUT_LIST_LEN)
        return; //can't do that Would hang cmdq
    cmd_queue_[cmd_queue_next_here_].read_num_numbers=num_args;
    cmd_queue_[cmd_queue_next_here_].function_ptr=fptr;
    cmd_queue_next_here_++;
    cmd_queue_next_here_ %= CMQ_QUEUE_LENGTH;
}

void cmdq_doWork(void)
{
    if ( cmd_queue_todo_pos_ == cmd_queue_next_here_ )
        return;

    // is num_args_read_ now equal to num args we expect ?
    if (num_args_read_ == cmd_queue_[cmd_queue_todo_pos_].read_num_numbers )
    {
        switch (num_args_read_)
        {
            case 0:
                ((void(*)(void)) cmd_queue_[cmd_queue_todo_pos_].function_ptr)();
                break;
            case 1:
                ((void(*)(int16_t)) cmd_queue_[cmd_queue_todo_pos_].function_ptr)(input_list_[0]);
                break;
            case 2:
                ((void(*)(int16_t,int16_t)) cmd_queue_[cmd_queue_todo_pos_].function_ptr)(input_list_[0], input_list_[1]);
                break;
        }
        num_args_read_ = 0;
        cmd_queue_todo_pos_++;
        cmd_queue_todo_pos_ %= CMQ_QUEUE_LENGTH;
    }
}