/*
ServoTest.cpp
(C)2013 kou029w - MIT License
*/

#include <Arduino.h>
#include <Servo.h>
#include "ServoTest.h"
#include "SerialUtil.h"

//#ifdef Servo_h
template<>
class ServoTestClass<Servo,HardwareSerial>;
#ifdef __AVR_ATmega32U4__
template<>
class ServoTestClass<Servo,Serial_>;
#endif
//#endif

template<typename SERVO, typename S>
void ServoTestClass<SERVO,S>::_setAndPrint(SERVO& servo, const char* servoName, S& serial){
  static unsigned long lastTime;
  lastTime = _time;
  _time = millis();
  serial.print("delay(");
  serial.print(_time - lastTime);
  serial.println(");");

  serial.print(servoName);
  switch(_mode){
  case ATTACH_MODE:
    serial.print(".attach(");
    serial.print(_val);
    servo.attach(_val);
    break;
  case DETACH_MODE:
    serial.println(".detach(");
    servo.detach();
    break;
  case WRITE_MODE:
    serial.print(".write(");
    serial.print(_val);
    servo.write(_val);
    break;
  case WRITE_MICROSEC_MODE:
    serial.print(".writeMicroseconds(");
    serial.print(_val);
    servo.writeMicroseconds(_val);
    break;
  }
  serial.println(");");
}

template<typename SERVO, typename S>
void ServoTestClass<SERVO,S>::operator()(S& serial){
  //  using namespace servotest;
  char _helpdoc[] = ""
    "/*\n"
    "## help ##\n"
    "default\n"
    "    Format : \"angle + 'a/b'\"\n"
    "    Example: \"90a\" => servoA.write(90)\n"
    "\n"
    "[t] attach\n"
    "    Format : \"'t' + pin + 'a/b'\"\n"
    "    Example: \"t8b\" => servoB.attach(8)\n"
    "\n"
    "[d] detach\n"
    "    Format : \"'d' + 'a/b'\"\n"
    "    Example: \"db\" => servoB.detach()\n"
    "\n"
    "[u] write-microseconds\n"
    "    Format : \"'u' + us + 'a/b'\"\n"
    "    Example: \"u1500a\" => servoA.writeMicroseconds(1500)\n"
    "*/\n";
  _time = millis();
  SERVO _servoA;
  SERVO _servoB;
  _servoA.attach(9);
  _servoB.attach(10);
  serial.println("servoA.attach(9);");
  serial.println("servoB.attach(10);");

  /* Main loop */
  for(;;){
    _mode = NONE;
    _val = 0;
    serial.println("/*-- [q] quit  [h] help --*/");
    while(_mode == NONE){
      char c = getStr(1, serial)[0];
      switch(c){
      case 'h':
        serial.print(_helpdoc);
        break;
      case 'q':
        _servoA.detach();
        _servoB.detach();
        return;
      case 't':
        _mode = ATTACH_MODE;
        break;
      case 'd':
        _mode = DETACH_MODE;
        break;
      case 'u':
        _mode = WRITE_MICROSEC_MODE;
        break;
      case '0'...'9':
        _val = _val * 10 + c - '0';
        break;
      case 'a':
        if(_mode == NONE) _mode = WRITE_MODE;
        _setAndPrint(_servoA, "servoA", serial);
        break;
      case 'b':
        if(_mode == NONE) _mode = WRITE_MODE;
        _setAndPrint(_servoB, "servoB", serial);
        break;
      }
    }
  }
}