//
//  Created by Arnaud Blanchard on 22/12/15.
//  Copyright ETIS 2015. All rights reserved.
//

#include "serial.h"
#include "blc_program.h"

#include <errno.h> //errno
#include <unistd.h>
#include <stdio.h> //getline
#include <stdlib.h> //strtol

char  const  *device_name;

int run=1;

void ask_quit(){
	run=0;
}

/*Scan string input as uchars. Return the number of char read. String address is updated with the current position*/
int sscan_uchars(char **string, uchar *uchars, int uchars_nb){
	int i, result;
	int value;
	int read_chars_nb;

	FOR(i, uchars_nb){
		result=sscanf(*string, "%d%n\n", &value, &read_chars_nb); //If there is no "\n" it does not matter
		if (result==1){
			if ((value < 0) || ( value > 254)) { //255 is a special value for synchronisation
				PRINT_WARNING("You need number in [0, 254] instead of '%ld' in %dth value", value, i);
				break;
			}
			else uchars[i]=value;
		}
		else break;
		*string +=read_chars_nb;
	}
	return i;
}

int mapping_ports(char const *string, int *ports, int ports_max){
	char const *current_pos=string;
	int port_id, i, read_chars_nb, result;

	FOR(i, ports_max){
		result=sscanf(current_pos, "%d%n", &port_id, &read_chars_nb);
		if (read_chars_nb==0) break;
		else if (result==0) EXIT_ON_ERROR("Wrong value '%s' for a port id", current_pos);
		else {
			if (port_id>ports_max) EXIT_ON_ERROR("Your port '%d' must be lower than '%d'", port_id, ports_max);
			ports[i] = port_id;
		}
		if (current_pos[0]==0) break;
		else current_pos+=read_chars_nb;
	}
	return i; //Number of mapped ports
}


int main(int argc, char **argv){
	blc_array array;
	char const *period_str;
	int period;
    char buffer[256];
    char *input_line=NULL;
    size_t input_line_capablity=0;
    blc_mem sending_mem, receiving_mem;
    char const *servos;
    char const  *baudrate_str;
    int  fd;
    size_t i;
    int  baudrate;
    int motors_nb=32; //ssc32
    int  ports_map[32];
    struct timeval timer;
    long int duration;

    
    ssize_t input_string_length;

    char* current_pos;
    size_t read_numbers_nb;
    
    blc_program_add_option(&baudrate_str, 'b', "baudrate", "integer",  "serial port speed", "115200");
    blc_program_add_option(&period_str, 'p', "period", "integer", "minimal period between two orders in ms", "0");
    blc_program_add_option(&servos, 's', "servos", "string",  "pins to drive servo motors \"pin1 pin2 pin3 ...\"", "0");
    blc_program_add_parameter(&device_name, "device", 0, "device name", "/dev/ttyUSB0");
    blc_program_init(&argc, &argv, NULL);
    
    baudrate=strtol(baudrate_str, NULL, 10);
   	motors_nb=mapping_ports(servos, ports_map, 32);
   	period=strtol(period_str, NULL, 10);

   	fprintf(stderr, "Mode mini SSC-II (compatible with SSC12, SSC32 and pololu)\n");

    fd = open_serial_port(device_name, baudrate, 8, 'N', 2);

    array.init('UIN8', 'NDEF', 1, motors_nb);
    i=0;

    blc_command_add("q", (type_blc_command_cb)ask_quit, NULL, NULL,  NULL);

    while(run==1){
    	blc_us_time_diff(&timer);

    	input_string_length=getline(&input_line, &input_line_capablity, stdin);

    	if (input_string_length==-1){
    		if (feof(stdin)) ask_quit();
    		else EXIT_ON_SYSTEM_ERROR("Getting input on stdin");
    	}
    	else if (input_string_length!=1){
    		if (input_line[0]=='#') continue; //It is a comment
    		current_pos=input_line;
    		read_numbers_nb=sscan_uchars(&current_pos, array.uchars, array.total_length);

    		//If it was not a number we try to interpret it as a command
    		if (current_pos==input_line) blc_command_interpret_string(input_line, input_string_length-1);
    		else if (read_numbers_nb != array.total_length) fprintf(stderr, "Wrong number of values. Waiting '%ld' values but reading '%ld'. Use for exemple option ' --servos=\"1 5 7\"' in order to drive only the motors 1,5,7\n", array.total_length, read_numbers_nb);
    		else if (current_pos < input_line + input_string_length-2) fprintf(stderr, "There is more text than needed for numbers.'%s' has not been interpreted\n", current_pos);
    		else {
    			//Format SSC2
				buffer[0]=255;
    			FOR(i, array.total_length){
    				buffer[1]=ports_map[i];
    				buffer[2]=array.uchars[i];
    				SYSTEM_SUCCESS_CHECK(write(fd, buffer, 3), 3,"Writing to the serial port");
    			}
    		}
        	duration=blc_us_time_diff(&timer);
        	if (duration < period) usleep(period*1000-duration);
    	}
    }
    return EXIT_SUCCESS;
}