#include "app.h"

#include <Arduino.h>
#include <HardwareSerial.h>

// http://bleaklow.com/2010/09/05/progmem_and_gcc_bug_34734.html
#undef PROGMEM
#define PROGMEM __attribute__((section(".progmem.data")))

#define GAUGE_COUNT 4

// See http://redbearlab.com/blendmicro/
struct gauge {
  uint8_t pin;
  uint8_t value;
} gauges[] = {
  { 9, 0},
  {10, 0},
  {11, 0},
  {13, 0}
};

void on_gauge_data(uint8_t *data, uint8_t len) {
  Serial.print(F("on_gauge_data, channel="));
  Serial.print(data[0], HEX);
  Serial.print(F(", data[1]="));
  Serial.println(data[1], HEX);
}

void on_gauge_ctrl(uint8_t *data, uint8_t len) {
  Serial.print(F("on_gauge_ctrl, data[0]="));
  Serial.print(data[0], HEX);
  Serial.print(F(", data[1]="));
  Serial.println(data[1], HEX);

  struct fsp_req *cmd = (struct fsp_req *) data;
  struct fsp_res res;

  res.code = cmd->code;

  Serial.print("code=");
  Serial.println(cmd->code, DEC);

  switch(cmd->code) {
    case FSP_CMD_GET_GAUGE_COUNT:
      res.len = sizeof(struct fsp_get_gauge_count_res);
      res.get_gauge_count.count = GAUGE_COUNT;
      break;
    case FSP_CMD_SET_GAUGE:
      res.len = sizeof(struct fsp_set_gauge_res);
      if (cmd->set_gauge.gauge < GAUGE_COUNT) {
        analogWrite(gauges[cmd->set_gauge.gauge].pin, cmd->set_gauge.value);
        gauges[cmd->set_gauge.gauge].value = cmd->set_gauge.value;
      }

      break;
    case FSP_CMD_GET_GAUGE:
      res.len = sizeof(struct fsp_get_gauge_res);
      if (cmd->get_gauge.gauge < GAUGE_COUNT) {
        res.get_gauge.gauge = cmd->get_gauge.gauge;
        res.get_gauge.value = gauges[cmd->get_gauge.gauge].value;
      } else {
        res.code = FSP_CMD_FAIL;
        res.len = 0;
      }
      break;
    default:
      res.code = FSP_CMD_FAIL;
      res.len = 0;
      break;
  }

  send_ctrl(&res);
}