/****************************/
/* マニピュレータの制御     */
/*      coded by Y.Suganuma */
/****************************/
#include  <stdio.h>
#include  <time.h>

/***************************/
/* xミリ秒経過するのを待つ */
/***************************/
int sleep(unsigned long x)
{
	clock_t  s = clock();
	clock_t  c;
	int sw = 1;

	do {
		if ((c = clock()) == (clock_t)-1) {       /* エラー */
			sw = 0;
			break;
		}
	} while (1000UL * (c - s) / CLOCKS_PER_SEC <= x); 

	return sw;
}

/*************************/
/* 入出力領域への入出力  */
/*      str1 : コピー先  */
/*      str2 : コピー元  */
/*      n : コピー文字数 */
/*      sw : 1 : 入力    */
/*           2 : 出力    */
/*************************/
void * memcpy(void *str1, void *str2, size_t n, int sw)
{
	int i1, k;
	unsigned char *c1 = (unsigned char *)str1, *c2 = (unsigned char *)str2;
	FILE *in, *out;
					// 入力
	if (sw == 1) {
		in = fopen("angle", "r");
		for (i1 = 0; i1 < (int)n; i1++) {
			fscanf(in, "%d", &k);
			c2[i1] = (unsigned char)k;
		}
		for (i1 = 0; i1 < (int)n; i1++)
			c1[i1] = c2[i1];
		fclose(in);
	}
					// 出力
	else {
		for (i1 = 0; i1 < (int)n; i1++)
			c1[i1] = c2[i1];
		out = fopen("angle", "w");
		k   = (int)c1[0];
		fprintf(out, "%d", k);
		for (i1 = 1; i1 < (int)n; i1++) {
			k = (int)c1[i1];
			fprintf(out, " %d", k);
		}
		fprintf(out, "\n");
		fclose(out);
	}

	return str1;
}

/********/
/* main */
/********/
int main(void)
{
    int k = 0, ang[3] = {0};
	unsigned char state[4], io[4];

	while (k < 3) {
		sleep(500);
		memcpy(state, io, 4, 1);
		if (state[0] == 1) {
			ang[0]   += 10;
			ang[1]   += 20;
			ang[2]   += 30;
			state[0]  = 0;
			state[1]  = (unsigned char)(255.0 * ang[0] / 90.0 + 0.5);
			state[2]  = (unsigned char)(255.0 * ang[1] / 90.0 + 0.5);
			state[3]  = (unsigned char)(255.0 * ang[2] / 90.0 + 0.5);
			memcpy(io, state, 4, 2);
			printf("angle %d %d %d\n", ang[0], ang[1], ang[2]);
			k++;
		}
		else if (state[0] > 1)
			break;
	}

    return 0;
}