#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include "scheduler.h"
#include "sequencer.h"
#include "psg.h"


const uint32_t CYCLE_TIME = 5; // ms

typedef enum {
  ACTION_RESULT_STAY,
  ACTION_RESULT_NEXT,
  ACTION_RESULT_REPEAT,
  ACTION_RESULT_END
} actionResult_t;

uint16_t taskId;



static actionResult_t _wait(uint32_t wait) {
  static bool running = false;
  static uint32_t counter = 0;

  actionResult_t res;
  if (! running) {
    running = true;
    counter = wait;
    res = ACTION_RESULT_STAY;
  } else {
    if (counter == 0) {
      running = false;
      res = ACTION_RESULT_NEXT;
    } else {
      counter -= 1;
      res = ACTION_RESULT_STAY;
    }
  }
  return res;
}

static actionResult_t _change_sound(uint16_t frequency_a, uint8_t volume_a, uint16_t frequency_b, uint8_t volume_b) {
  // set frequency_a and volume_a to generator 0
  psgPlayFrequency(0, 0, volume_a, frequency_a);
  // set frequency_b and volume_b to generator 1
  psgPlayFrequency(1, 0, volume_b, frequency_b);
  return ACTION_RESULT_NEXT;
}

static bool _change_volume(uint8_t volume_a, uint8_t volume_b) {
  // set volume_a to generator 0
  psgAmplitude(0, 0, volume_a);
  // set volume_b to generator 1
  psgAmplitude(1, 0, volume_b);
  return ACTION_RESULT_NEXT;
}

void sequencerExec(void *handle) {
  sequencerAction_t *seq = (sequencerAction_t*) handle;

  static uint16_t seqIdx = 0;

  sequencerAction_t *action = seq + seqIdx;

  actionResult_t res;
  switch (action->type) {
    case SA_WAIT:
      res = _wait(action->u.wait);
      break;
    case SA_REPEAT:
      res = ACTION_RESULT_REPEAT;
      break;
    case SA_END:
      res = ACTION_RESULT_END;
      break;
    case SA_CHANGE_SOUND:
      res = _change_sound(
          action->u.cs.frequency_a,
          action->u.cs.volume_a,
          action->u.cs.frequency_b,
          action->u.cs.volume_b);
      break;
    case SA_CHANGE_VOLUME:
      res = _change_volume(
          action->u.cs.volume_a,
          action->u.cs.volume_b);
      break;
    default:
      break;
  }

  switch (res) {
    case ACTION_RESULT_NEXT:
      seqIdx += 1;
      break;
    case ACTION_RESULT_REPEAT:
      seqIdx = 0;
      break;
    case ACTION_RESULT_STAY:
      break;
    case ACTION_RESULT_END:
      schDel(taskId);
      break;
  } 
}

void sequencerStart(const sequencerAction_t *sequence) {
  taskId = schAdd(sequencerExec, (void*) sequence, 0, CYCLE_TIME);
}

void sequencerInit() {
}