#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include "dvbfe.h"
#include "sec.h"

int dvbfe_sec_set(struct dvbfe_handle *fe,
		   struct dvbfe_sec_config *sec_config,
		   enum dvbfe_diseqc_switch sat_pos,
		   enum dvbfe_diseqc_switch switch_option,
		   struct dvbfe_parameters *params,
		   int timeout)
{
	int tmp;
	struct dvbfe_parameters localparams;
	struct dvbfe_parameters *topass = params;

	// perform SEC
	if (sec_config != NULL) {
		switch(sec_config->config_type) {
		case DVBFE_SEC_CONFIG_NONE:
			break;
		case DVBFE_SEC_CONFIG_SIMPLE:
		{
			// calculate the correct oscillator value
			enum dvbfe_diseqc_oscillator osc = DISEQC_OSCILLATOR_LOW;
			if (sec_config->switch_frequency && (sec_config->switch_frequency < params->frequency))
				osc = DISEQC_OSCILLATOR_HIGH;

			if ((tmp = dvbfe_sec_std_sequence(fe,
			     				  osc,
							  params->u.dvbs.polarization,
							  sat_pos,
							  switch_option)) < 0)
				return tmp;
			break;
		}

		case DVBFE_SEC_CONFIG_ADVANCED:
		{
			// are we high or not?
			int high = 0;
			if (sec_config->switch_frequency && (sec_config->switch_frequency < params->frequency))
				high = 1;

			//  determine correct string
			char *cmd = NULL;
			switch(params->u.dvbs.polarization) {
			case DVBFE_POLARIZATION_H:
				if (!high)
					cmd = sec_config->adv_cmd_lo_h;
				else
					cmd = sec_config->adv_cmd_hi_h;
				break;
			case DVBFE_POLARIZATION_V:
				if (!high)
					cmd = sec_config->adv_cmd_lo_v;
				else
					cmd = sec_config->adv_cmd_hi_v;
				break;
			case DVBFE_POLARIZATION_L:
				if (!high)
					cmd = sec_config->adv_cmd_lo_l;
				else
					cmd = sec_config->adv_cmd_hi_l;
				break;
			case DVBFE_POLARIZATION_R:
				if (!high)
					cmd = sec_config->adv_cmd_lo_r;
				else
					cmd = sec_config->adv_cmd_hi_r;
				break;
			default:
				return -EINVAL;
			}

			// do it
			if (cmd)
				if ((tmp = dvbfe_sec_command(fe, cmd)) < 0)
					return tmp;
			break;
		}
		}

		// frequency adjustment
		uint32_t lof = sec_config->lof_lo;
		if (sec_config->switch_frequency && (sec_config->switch_frequency < params->frequency))
			lof = sec_config->lof_hi;

		if (lof) {
			memcpy(&localparams, params, sizeof(struct dvbfe_parameters));
			localparams.frequency -= lof;
			topass = &localparams;
		}
	}

	// set the frontend!
	return dvbfe_set(fe, topass, timeout);
}

int dvbfe_sec_std_sequence(struct dvbfe_handle *fe,
			   enum dvbfe_diseqc_oscillator oscillator,
			   enum dvbfe_polarization polarization,
			   enum dvbfe_diseqc_switch sat_pos,
			   enum dvbfe_diseqc_switch switch_option)
{
	dvbfe_set_22k_tone(fe, DVBFE_SEC_TONE_OFF);

	switch(polarization) {
	case DVBFE_POLARIZATION_V:
	case DVBFE_POLARIZATION_R:
		dvbfe_set_voltage(fe, DVBFE_SEC_VOLTAGE_13);
		break;
	case DVBFE_POLARIZATION_H:
	case DVBFE_POLARIZATION_L:
		dvbfe_set_voltage(fe, DVBFE_SEC_VOLTAGE_18);
		break;
	default:
		return -EINVAL;
	}

	dvbfe_diseqc_set_committed_switches(fe,
					    DISEQC_ADDRESS_ANY_DEVICE,
					    oscillator,
					    polarization,
					    sat_pos,
					    switch_option);

	usleep(15000);

	switch(sat_pos) {
	case DISEQC_SWITCH_A:
		dvbfe_set_tone_data_burst(fe, DVBFE_SEC_MINI_A);
		break;
	case DISEQC_SWITCH_B:
		dvbfe_set_tone_data_burst(fe, DVBFE_SEC_MINI_B);
		break;
	default:
		break;
	}

	if (sat_pos != DISEQC_SWITCH_UNCHANGED)
		usleep(15000);

	switch(oscillator) {
	case DISEQC_OSCILLATOR_LOW:
		dvbfe_set_22k_tone(fe, DVBFE_SEC_TONE_OFF);
		break;
	case DISEQC_OSCILLATOR_HIGH:
		dvbfe_set_22k_tone(fe, DVBFE_SEC_TONE_ON);
		break;
	default:
		break;
	}

	return 0;
}

int dvbfe_diseqc_set_reset(struct dvbfe_handle *fe,
			   enum dvbfe_diseqc_address address,
			   enum dvbfe_diseqc_reset state)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x00 };

	if (state == DISEQC_RESET_CLEAR)
		data[2] = 0x01;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_power(struct dvbfe_handle *fe,
			   enum dvbfe_diseqc_address address,
			   enum dvbfe_diseqc_power state)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x02 };

	if (state == DISEQC_POWER_ON)
		data[2] = 0x03;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_listen(struct dvbfe_handle *fe,
			    enum dvbfe_diseqc_address address,
			    enum dvbfe_diseqc_listen state)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x30 };

	if (state == DISEQC_LISTEN_AWAKE)
		data[2] = 0x31;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_committed_switches(struct dvbfe_handle *fe,
					enum dvbfe_diseqc_address address,
					enum dvbfe_diseqc_oscillator oscillator,
					int polarization,
					enum dvbfe_diseqc_switch sat_pos,
					enum dvbfe_diseqc_switch switch_option)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x38, 0x00 };

	switch(oscillator) {
	case DISEQC_OSCILLATOR_LOW:
		data[3] |= 0x10;
		break;
	case DISEQC_OSCILLATOR_HIGH:
		data[3] |= 0x01;
		break;
	case DISEQC_OSCILLATOR_UNCHANGED:
		break;
	}
	switch(polarization) {
	case DVBFE_POLARIZATION_V:
	case DVBFE_POLARIZATION_R:
		data[3] |= 0x20;
		break;
	case DVBFE_POLARIZATION_H:
	case DVBFE_POLARIZATION_L:
		data[3] |= 0x02;
		break;
	case -1:
		break;
	}
	switch(sat_pos) {
	case DISEQC_SWITCH_A:
		data[3] |= 0x40;
		break;
	case DISEQC_SWITCH_B:
		data[3] |= 0x04;
		break;
	case DISEQC_SWITCH_UNCHANGED:
		break;
	}
	switch(switch_option) {
	case DISEQC_SWITCH_A:
		data[3] |= 0x80;
		break;
	case DISEQC_SWITCH_B:
		data[3] |= 0x08;
		break;
	case DISEQC_SWITCH_UNCHANGED:
		break;
	}

	if (data[3] == 0)
		return 0;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_uncommitted_switches(struct dvbfe_handle *fe,
					  enum dvbfe_diseqc_address address,
					  enum dvbfe_diseqc_switch s1,
					  enum dvbfe_diseqc_switch s2,
					  enum dvbfe_diseqc_switch s3,
					  enum dvbfe_diseqc_switch s4)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x39, 0x00 };

	switch(s1) {
	case DISEQC_SWITCH_A:
		data[3] |= 0x10;
		break;
	case DISEQC_SWITCH_B:
		data[3] |= 0x01;
		break;
	case DISEQC_SWITCH_UNCHANGED:
		break;
	}
	switch(s2) {
	case DISEQC_SWITCH_A:
		data[3] |= 0x20;
		break;
	case DISEQC_SWITCH_B:
		data[3] |= 0x02;
		break;
	case DISEQC_SWITCH_UNCHANGED:
		break;
	}
	switch(s3) {
	case DISEQC_SWITCH_A:
		data[3] |= 0x40;
		break;
	case DISEQC_SWITCH_B:
		data[3] |= 0x04;
		break;
	case DISEQC_SWITCH_UNCHANGED:
		break;
	}
	switch(s4) {
	case DISEQC_SWITCH_A:
		data[3] |= 0x80;
		break;
	case DISEQC_SWITCH_B:
		data[3] |= 0x08;
		break;
	case DISEQC_SWITCH_UNCHANGED:
		break;
	}

	if (data[3] == 0)
		return 0;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_analog_value(struct dvbfe_handle *fe,
				  enum dvbfe_diseqc_address address,
				  enum dvbfe_diseqc_analog_id id,
				  uint8_t value)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x48, value };

	if (id == DISEQC_ANALOG_ID_A1)
		data[2] = 0x49;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_frequency(struct dvbfe_handle *fe,
			       enum dvbfe_diseqc_address address,
			       uint32_t frequency)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x58, 0x00, 0x00, 0x00 };
	int len = 5;

	uint32_t bcdval = 0;
	int i;
	for(i=0; i<=24;i+=4) {
		bcdval |= ((frequency % 10) << i);
		frequency /= 10;
	}

	data[3] = bcdval >> 16;
	data[4] = bcdval >> 8;
	if (bcdval & 0xff) {
		data[5] = bcdval;
		len++;
	}

	return dvbfe_do_diseqc_command(fe, data, len);
}

int dvbfe_diseqc_set_channel(struct dvbfe_handle *fe,
			     enum dvbfe_diseqc_address address,
			     uint16_t channel)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x59, 0x00, 0x00};

	data[3] = channel >> 8;
	data[4] = channel;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_halt_satpos(struct dvbfe_handle *fe,
			     enum dvbfe_diseqc_address address)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x60};

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_disable_satpos_limits(struct dvbfe_handle *fe,
				       enum dvbfe_diseqc_address address)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x63};

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_set_satpos_limit(struct dvbfe_handle *fe,
				  enum dvbfe_diseqc_address address,
				  enum dvbfe_diseqc_direction direction)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x66};

	if (direction == DISEQC_DIRECTION_WEST)
		data[2] = 0x67;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_drive_satpos_motor(struct dvbfe_handle *fe,
				    enum dvbfe_diseqc_address address,
				    enum dvbfe_diseqc_direction direction,
				    enum dvbfe_diseqc_drive_mode mode,
				    uint8_t value)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x68, 0x00};

	if (direction == DISEQC_DIRECTION_WEST)
		data[2] = 0x69;

	switch(mode) {
	case DISEQC_DRIVE_MODE_STEPS:
		data[3] = (value & 0x7f) | 0x80;
		break;
	case DISEQC_DRIVE_MODE_TIMEOUT:
		data[3] = value & 0x7f;
		break;
	}

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_store_satpos_preset(struct dvbfe_handle *fe,
				     enum dvbfe_diseqc_address address,
				     uint8_t id)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x6A, id};

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_goto_satpos_preset(struct dvbfe_handle *fe,
				    enum dvbfe_diseqc_address address,
				    uint8_t id)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x6B, id};

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

int dvbfe_diseqc_recalculate_satpos_positions(struct dvbfe_handle *fe,
					      enum dvbfe_diseqc_address address,
					      int val1,
					      int val2)
{
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x6F, 0x00, 0x00};
	int len = 3;

	if (val1 != -1) {
		data[3] = val1;
		len++;
	}
	if (val2 != -1) {
		data[4] = val2;
		len = 5;
	}

	return dvbfe_do_diseqc_command(fe, data, len);
}

int dvbfe_diseqc_goto_rotator_bearing(struct dvbfe_handle *fe,
				      enum dvbfe_diseqc_address address,
				      float angle)
{
	int integer = (int) angle;
	uint8_t data[] = { DISEQC_FRAMING_MASTER_NOREPLY, address, 0x6e, 0x00, 0x00};

	// transform the fraction into the correct representation
	int fraction = (int) (((angle - integer) * 16.0) + 0.9) & 0x0f;
	switch(fraction) {
	case 1:
	case 4:
	case 7:
	case 9:
	case 12:
	case 15:
		fraction--;
		break;
	}

	// generate the command
	if (integer < -256) {
		return -EINVAL;
	} else if (integer < 0) {
		integer = -integer;
		data[3] = 0xf0;
	} else if (integer < 256) {
		data[3] = 0x00;
	} else if (integer < 512) {
		integer -= 256;
		data[3] = 0x10;
	} else {
		return -EINVAL;
	}
	data[3] |= ((integer / 16) & 0x0f);
	integer = integer % 16;
	data[4] |= ((integer & 0x0f) << 4) | fraction;

	return dvbfe_do_diseqc_command(fe, data, sizeof(data));
}

static int skipwhite(char **line, char *end)
{
	while(**line) {
		if (end && (*line >= end))
			return -1;
		if (!isspace(**line))
			return 0;
		*line++;
	}

	return -1;
}

static int getstringupto(char **line, char *end, char *chars, char **ptrdest, int *ptrlen)
{
	char *start = *line;

	while(**line) {
		if (end && (*line >= end))
			return -1;
		if (strchr(chars, **line)) {
			*ptrdest = start;
			*ptrlen = *line - start;
			return 0;
		}
		*line++;
	}

	return -1;
}

static int parsefunction(char **line,
			 char **nameptr, int *namelen,
			 char **argsptr, int *argslen)
{
	if (skipwhite(line, NULL))
		return -1;

	if (getstringupto(line, NULL, "(", nameptr, namelen))
		return -1;
	if (getstringupto(line, NULL, ")", argsptr, argslen))
		return -1;

	return 0;
}

static int parseintarg(char **args, char *argsend, int *result)
{
	char tmp[32];
	char *arg;
	int arglen;

	// skip whitespace
	if (skipwhite(args, argsend))
		return -1;

	// get the arg
	if (getstringupto(args, argsend, ",)", &arg, &arglen))
		return -1;
	if (arglen > 31)
		arglen = 31;
	strncpy(tmp, arg, arglen);
	arg[arglen] = 0;

	if (sscanf(tmp, "%i", result) != 1)
		return -1;

	return 0;
}

static int parsechararg(char **args, char *argsend, int *result)
{
	char *arg;
	int arglen;

	// skip whitespace
	if (skipwhite(args, argsend))
		return -1;

	// get the arg
	if (getstringupto(args, argsend, ",)", &arg, &arglen))
		return -1;
	if (arglen > 0)
		*result = arg[0];

	return 0;
}

static int parsefloatarg(char **args, char *argsend, float *result)
{
	char tmp[32];
	char *arg;
	int arglen;

	// skip whitespace
	if (skipwhite(args, argsend))
		return -1;

	// get the arg
	if (getstringupto(args, argsend, ",)", &arg, &arglen))
		return -1;
	if (arglen > 31)
		arglen = 31;
	strncpy(tmp, arg, arglen);
	arg[arglen] = 0;

	if (sscanf(tmp, "%f", result) != 1)
		return -1;

	return 0;
}

int dvbfe_sec_command(struct dvbfe_handle *fe, char *command)
{
	char *name;
	char *args;
	int namelen;
	int argslen;
	int address;
	int iarg;
	int iarg2;
	int iarg3;
	int iarg4;
	float farg;

	while(parsefunction(&command, &name, &namelen, &args, &argslen)) {
		if (!strcmp(name, "tone")) {
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			if (iarg) {
				dvbfe_set_22k_tone(fe, DVBFE_SEC_TONE_ON);
			} else {
				dvbfe_set_22k_tone(fe, DVBFE_SEC_TONE_OFF);
			}
		} else if (!strcmp(name, "voltage")) {
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			switch(iarg) {
			case 0:
				dvbfe_set_voltage(fe, DVBFE_SEC_VOLTAGE_OFF);
				break;
			case 13:
				dvbfe_set_voltage(fe, DVBFE_SEC_VOLTAGE_13);
				break;
			case 18:
				dvbfe_set_voltage(fe, DVBFE_SEC_VOLTAGE_18);
				break;
			default:
				return -1;
			}
		} else if (!strcmp(name, "toneburst")) {
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			if (iarg) {
				dvbfe_set_tone_data_burst(fe, DVBFE_SEC_MINI_B);
			} else {
				dvbfe_set_tone_data_burst(fe, DVBFE_SEC_MINI_A);
			}
		} else if (!strcmp(name, "highvoltage")) {
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			dvbfe_set_high_lnb_voltage(fe, iarg ? 1 : 0);
		} else if (!strcmp(name, "dishnetworks")) {
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			dvbfe_do_dishnetworks_legacy_command(fe, iarg);
		} else if (!strcmp(name, "wait")) {
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			if (iarg)
				usleep(iarg * 1000);
		} else if (!strcmp(name, "Dreset")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			if (iarg) {
				dvbfe_diseqc_set_reset(fe, address, DISEQC_RESET);
			} else {
				dvbfe_diseqc_set_reset(fe, address, DISEQC_RESET_CLEAR);
			}
		} else if (!strcmp(name, "Dpower")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			if (iarg) {
				dvbfe_diseqc_set_power(fe, address, DISEQC_POWER_ON);
			} else {
				dvbfe_diseqc_set_power(fe, address, DISEQC_POWER_OFF);
			}
		} else if (!strcmp(name, "Dcommitted")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parsechararg(&args, args+argslen, &iarg))
				return -1;
			if (parsechararg(&args, args+argslen, &iarg2))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg3))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg4))
				return -1;
			// FIXME
		} else if (!strcmp(name, "Duncommitted")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg2))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg3))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg4))
				return -1;

			dvbfe_diseqc_set_uncommitted_switches(fe, address,
								iarg ? DISEQC_SWITCH_A : DISEQC_SWITCH_B,
								iarg2 ? DISEQC_SWITCH_A : DISEQC_SWITCH_B,
								iarg3 ? DISEQC_SWITCH_A : DISEQC_SWITCH_B,
								iarg4 ? DISEQC_SWITCH_A : DISEQC_SWITCH_B);
		} else if (!strcmp(name, "Dfrequency")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			dvbfe_diseqc_set_frequency(fe, address, iarg);
		} else if (!strcmp(name, "Dchannel")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			dvbfe_diseqc_set_channel(fe, address, iarg);
		} else if (!strcmp(name, "Dgotopreset")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parseintarg(&args, args+argslen, &iarg))
				return -1;

			dvbfe_diseqc_goto_satpos_preset(fe, address, iarg);
		} else if (!strcmp(name, "Dgotobearing")) {
			if (parseintarg(&args, args+argslen, &address))
				return -1;
			if (parsefloatarg(&args, args+argslen, &farg))
				return -1;

			dvbfe_diseqc_goto_rotator_bearing(fe, address, farg);
		} else {
			return -1;
		}
	}

	return 0;
}