/*
 * rescurerobo.c
 *   タミヤレスキューロボット無線化
 */

#include  <stdio.h>
#include  <string.h>
#include  <stdlib.h>
#include  <unistd.h>
#include  <sys/types.h>
#include  <sys/socket.h>
#include  <netinet/in.h>  /* #include < sys/un.h >の代わり */
#include  <wiringPi.h>

/* モーター制御ピンを定義 */
#define M1A 2
#define M1B 3
#define M2A 27
#define M2B 10
#define M3A 9
#define M3B 11
#define MOTORPINS 6	/* 6本 */

/* LED PIN */
#define LEDP 24
#define LEDPINS 1       /* 1本 */ 

/* PWMピンを定義 */
#define PWMPIN 18	/* 18番 */
#define PWMPINS 1	/* 1本 */

/* サーボ制御を定義 */
#define SERVOPINS 2	/* 2本 */
#define SERVODIVN  3	/* 動作範囲 倍率分子 */
#define SERVODIVD  4	/* 動作範囲 倍率分母 */

#define PORT 8765
/* デバッグ用データダンプ */
void datadump( char *ca ) {
	int i;
	for ( i=0 ; i<strlen(ca); i++ ) {
		printf("%02X ",ca[i]);
	}
	printf("\n");
}


/* ServoBlaseter対応版 */
void servovalue(int sn , int val ) {
	FILE *fp;
	static char str[128];

	fp=fopen("/dev/servoblaster","w");
	if ( fp == NULL ) {exit;}
	sprintf (str ,"%d=%d\n" ,sn ,val);
	//printf("<%s>",str);
	if (-1 == fputs(str , fp)) {
		printf("fputs error\n");
	}
	fclose (fp);
}

int main(int argc , char *argv[])
{
    int    i;
    int    fd1, fd2;
    struct sockaddr_in    saddr;
    struct sockaddr_in    caddr;
    int    len;
    int    ret;
    char   buf[1024];
    char   s[16];
    int    result;
    int	   port;
    char   *data;
    int    dati;
    int	   motor[]={M1A,M1B,M2A,M2B,M3A,M3B};

    port=PORT;	/* 仮のポートナンバー */
    /* 引数の処理 */
    while (( result=getopt(argc,argv,"p:")) != -1 ) {
	switch (result) {
	case 'p':
	    port=atoi(optarg);
	    break;
	default:
	    exit(1);
	    break;
	}
    }

    /* wiringPiの初期化 */	
    if (wiringPiSetupGpio() == -1 ) return 1;

    /* GPIOの準備 */
    pinMode(M1A, OUTPUT);
    pinMode(M1B, OUTPUT);
    pinMode(M2A, OUTPUT);
    pinMode(M2B, OUTPUT);
    pinMode(M3A, OUTPUT);
    pinMode(M3B, OUTPUT);
    pinMode(LEDP, OUTPUT);

    for ( i=0 ; i< MOTORPINS ; i++ ) {
	digitalWrite(motor[i], 0);	/* 初期値はオール0 */
    }
    digitalWrite(LEDP, 0);	/* LED=OFF */

    /* PWMの準備 */
	pinMode(PWMPIN, PWM_OUTPUT);

    /* ソケットを作る。このソケットはUNIXドメインで、DGRAM型ソケット。*/
    if ((fd1 = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
	perror("socket");
	exit(1);
    }

    /* saddrの中身を0にしておかないと、bind()でエラーが起こることがある */
    bzero((char *)&saddr, sizeof(saddr));

    /* ソケットの名前を入れておく */
    saddr.sin_family = AF_INET;
    saddr.sin_addr.s_addr = INADDR_ANY;
    saddr.sin_port = htons(port);

    /* ソケットにアドレスをバインドする。*/
    if (bind(fd1, (struct sockaddr *)&saddr, sizeof(saddr)) < 0){
	perror("bind");
	exit(1);
    }
    
    len = sizeof(caddr);

    while (1) {
	/* recvfromでクライアントからのデータを受信。 */
	if ((ret = recvfrom(fd1, buf, 1024, 0, (struct sockaddr *)&caddr, &len)) < 0){
	    perror("recvfrom");
	    exit(1);
	}

	//datadump(buf);	/* for DEBUG */
	//printf("%s\n",buf);	/* for DEBUG */

	/* チャンネル毎のループ。*/
	for (i=0 ; i < MOTORPINS+PWMPINS+SERVOPINS+LEDPINS ; i++ ) {
		if ( i==0 ) {
		    data=strtok(buf,",");
		} else {
		    data=strtok(NULL,",");
		}

		if ( data==NULL ) {break;}

		//printf ("=%s=",data);	/* for DEBUG */

		dati = atoi(data);
		if ( i < MOTORPINS ) {
			/* モーターピンの場合 */
			if ( 0 <= dati || dati <= 1 ) {
				digitalWrite(motor[i], dati);
			}
		} else if ( i < MOTORPINS+PWMPINS ) {
			if ( 0 <= dati || dati < 256) {
				pwmWrite(PWMPIN ,dati*4);
			}
		} else if ( i < MOTORPINS+PWMPINS+SERVOPINS ) {
			/* サーボピンの場合 */
			if ( 0 <= dati || dati < 256 ) {
				dati=dati*SERVODIVN/SERVODIVD;
				dati+=150-128*SERVODIVN/SERVODIVD;
				if (dati<75) {dati=75;}
				if (225<dati) {dati=225;}
				servovalue(i-MOTORPINS-PWMPINS,dati);
			}
		} else {
			/* LEDピンの場合 */
			if ( 0 <= dati || dati <= 1 ) {
				digitalWrite(LEDP, dati);
			}
		}
	}
	//printf ("\n");	/* for DEBUG */

	strcpy ( buf , "> ");
	ret=strlen(buf);

	if (sendto(fd1, buf , ret, 0, (struct sockaddr *)&caddr, len) != ret){
	    perror("sendto");
	    exit(1);
	}
    }
}