/*
 * サーボ・ハード関連
 *
 * Copyright (c) 2016 Wakayama.rb Ruby Board developers
 *
 * This software is released under the MIT License.
 * https://github.com/wakayamarb/wrbb-v2lib-firm/blob/master/MITL
 *
 */
#include <arduino.h>
/*#include <servo.h>*/

#include <mruby.h>
/*#include <mruby/string.h>*/
/*#include <mruby/variable.h>*/

#include "../llbruby.h"
#include "sKernel.h"

#define ATTACH_MAX	12
struct pin_node *servo[ATTACH_MAX];

/******************************************************/
/* サーボ出力を任意のピンに割り当てます: Servo.attach*/
/* Servo.attach(ch, pin[,min,max])*/
/*	ch: サーボのチャネル 0～11まで指定できます*/
/*  pin: 割り当てるピン番号*/
/*  min: サーボの角度が0度のときのパルス幅(マイクロ秒)。デフォルトは544*/
/*  max: サーボの角度が180度のときのパルス幅(マイクロ秒)。デフォルトは2400*/
/******************************************************/
mrb_value mrb_servo_attach(mrb_state *mrb, mrb_value self)
{
	int ch;
	int pinno;
	int min;
	int max;
	struct pin_node *pin;

	int n = mrb_get_args(mrb, "ii|ii", &ch, &pinno, &min, &max);

	if (pinno >= 20) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	/* nが2以下のときは、minが引数に無いので、 */
	if (n <= 2) {
		min = 544;
	}

	/* nが3以下のときは、maxが引数に無いので、 */
	if (n <= 3) {
		max = 2400;
	}

	if (ch < 0 || ch >= ATTACH_MAX) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	if (servo[ch] != 0) {
		pwmout_free(&servo[ch]->pwmout);
		/*servo[ch] = 0;*/
	}

	pin = get_pin(pintype_pwmout, pinno);
	if (pin == NULL)
		return mrb_nil_value();			/*戻り値は無しですよ。*/

	servo[ch] = pin;
	servo[ch]->min = min;
	servo[ch]->max = max;

	return mrb_nil_value();			/*戻り値は無しですよ。*/
}

/******************************************************/
/* サーボの角度をセットします: Servo.write*/
/* Servo.write(ch,angle)*/
/*	ch: サーボのチャネル 0～11まで指定できます*/
/*  angle: 角度 0～180*/
/******************************************************/
mrb_value mrb_servo_write(mrb_state *mrb, mrb_value self)
{
	int ch;
	int angle;
	int min;
	int max;

	mrb_get_args(mrb, "ii", &ch, &angle);

	if (ch < 0 || ch >= ATTACH_MAX) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	if (servo[ch] == NULL) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	DEBUG_PRINT("Servo", angle);

	min = servo[ch]->min;
	max = servo[ch]->max;
	angle *= servo[ch]->period;

	pwmout_pulsewidth_us(&servo[ch]->pwmout, (angle * (max - min)) / 180 + min);

	return mrb_nil_value();			/*戻り値は無しですよ。*/
}

/******************************************************/
/* サーボモータにus単位で角度を指定する: Servo.us*/
/* Servo.us(ch,us)*/
/*	ch: サーボのチャネル 0～11まで指定できます*/
/*  us: 出力したいパルスの幅 1～19999, 0で出力 OFF*/
/*   サーボモータに与えられるパルスは20ms周期で、1周期中のHighの時間を直接指定する。*/
/*   実質的にPWM出力。連続回転タイプのサーボでは、回転のスピードが設定することができる。*/
/******************************************************/
mrb_value mrb_servo_us(mrb_state *mrb, mrb_value self)
{
	int ch;
	int us;

	mrb_get_args(mrb, "ii", &ch, &us);

	if (ch < 0 || ch >= ATTACH_MAX) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	if (servo[ch] == 0) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	pwmout_pulsewidth_us(&servo[ch]->pwmout, us);

	return mrb_nil_value();			/*戻り値は無しですよ。*/
}

/******************************************************/
/* 最後に設定された角度を読み出す: Servo.read*/
/* Servo.read(ch)*/
/*	ch: サーボのチャネル 0～11まで指定できます*/
/* 戻り値*/
/*  マイクロ秒単位 us(ch) で与えた値は読みとれない*/
/******************************************************/
mrb_value mrb_servo_read(mrb_state *mrb, mrb_value self)
{
	int ch;
	int ret = 0;

	mrb_get_args(mrb, "i", &ch);

	if (ch < 0 || ch >= ATTACH_MAX) {
		return mrb_fixnum_value(ret);
	}

	if (servo[ch] == 0) {
		return mrb_fixnum_value(ret);
	}

	ret = servo[ch]->pulsewidth;

	return mrb_fixnum_value(ret);
}

/******************************************************/
/* ピンにサーボが割り当てられているかを確認する: Servo.attached*/
/* Servo.attached(ch)*/
/*	ch: サーボのチャネル 0～11まで指定できます*/
/* 戻り値*/
/*  1: 割り当てられている*/
/*  0: 割り当てはない*/
/******************************************************/
mrb_value mrb_servo_attached(mrb_state *mrb, mrb_value self)
{
	int ch;
	int ret = 0;

	mrb_get_args(mrb, "i", &ch);

	if (ch < 0 || ch >= ATTACH_MAX) {
		return mrb_fixnum_value(ret);
	}

	if (servo[ch] == 0) {
		return mrb_fixnum_value(ret);
	}

	if (get_pin_type(servo[ch]->pin) == pintype_pwmout) {
		ret = 1;
	}

	return mrb_fixnum_value(ret);
}

/******************************************************/
/* サーボの動作を止め、割り込みを禁止する: Servo.detach*/
/* Servo.detach(ch)*/
/*	ch: サーボのチャネル 0～9まで指定できます*/
/******************************************************/
mrb_value mrb_servo_detach(mrb_state *mrb, mrb_value self)
{
	int ch;

	mrb_get_args(mrb, "i", &ch);

	if (ch < 0 || ch >= ATTACH_MAX) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	if (servo[ch] == NULL) {
		return mrb_nil_value();			/*戻り値は無しですよ。*/
	}

	pwmout_free(&servo[ch]->pwmout);
	servo[ch] = 0;

	return mrb_nil_value();			/*戻り値は無しですよ。*/
}

/******************************************************/
/* ライブラリを定義します*/
/******************************************************/
void servo_Init(mrb_state *mrb)
{
	for (int i = 0; i < ATTACH_MAX; i++) {
		servo[i] = 0;
	}

	struct RClass *servoModule = mrb_define_module(mrb, "Servo");

	mrb_define_module_function(mrb, servoModule, "attach", mrb_servo_attach, MRB_ARGS_REQ(2) | MRB_ARGS_OPT(2));
	mrb_define_module_function(mrb, servoModule, "write", mrb_servo_write, MRB_ARGS_REQ(2));
	mrb_define_module_function(mrb, servoModule, "us", mrb_servo_us, MRB_ARGS_REQ(2));
	mrb_define_module_function(mrb, servoModule, "read", mrb_servo_read, MRB_ARGS_REQ(1));
	mrb_define_module_function(mrb, servoModule, "attached", mrb_servo_attached, MRB_ARGS_REQ(1));
	mrb_define_module_function(mrb, servoModule, "detach", mrb_servo_detach, MRB_ARGS_REQ(1));
}