/*
 * Libswitchhw-tester
 *
 */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "switch_hw.h"

//#include "pio_pins.h"
#include "pio.h"



#include <sys/ioctl.h>
#include <at91_softpwm.h>
#include "fan.h"
#include <fcntl.h>

extern const pio_pin_t PIN_mbl_box_fan_en[];
static int pwm_fd;

//extern const pio_pin_t* _all_cpu_gpio_pins[];
//extern pio_pin_t* _all_fpga_gpio_pins[];

//--------------------------------------- functions

void usage_error(char **argv)
{
	fprintf(stderr, "usage: %s <command> <values>\n", argv[0]);
	fprintf(stderr, "	--fan <fan_speed> (from 0 to 100)\n");
	fprintf(stderr, "	--led <test_ID> \n");
	exit(1);
}


/**
 * Copy of the function in fan.c to control the softpwn driver.
 */
static int enable_d0 = 0;
void pwm_configure_pin2(const pio_pin_t *pin, int enable, int rate)
{
	int index = pin->port * 32 + pin->pin;

	if(enable && !enable_d0)
		ioctl(pwm_fd, AT91_SOFTPWM_ENABLE, index);
	else if(!enable && enable_d0)
		ioctl(pwm_fd, AT91_SOFTPWM_DISABLE, index);

	enable_d0 = enable;
	ioctl(pwm_fd, AT91_SOFTPWM_SETPOINT, rate);
}


void shw_pio_printf(const pio_pin_t *pio)
{
	printf("PIO_%c(%d) mode=%d dir=%s\t",
			(char)((int)'A'+pio->port-1), pio->pin, pio->mode, ((pio->dir>0)?"OUT":"IN"));
	if(pio->dir==0) printf(" => %d\n", shw_pio_get(pio));
	//else printf(" x=> %x\n", (_readl(_pio_base[3][pio->port])));
	else printf("\n");
}


void fan_speed(int speed)
{
	const pio_pin_t* fan_en = (const pio_pin_t *)&PIN_mbl_box_fan_en;
	TRACE(TRACE_INFO,"Starting Fan Test at %d%", speed);

	shw_pio_mmap_init();
	shw_pio_configure(fan_en);

	pwm_fd = open("/dev/at91_softpwm", O_RDWR);
	if(pwm_fd < 0)
	{
		TRACE(TRACE_INFO, "at91_softpwm driver not installed or device not created (Only on/off mode).");
		if(speed<=0) 		shw_pio_set(fan_en,0);
		else if(speed>=100)	shw_pio_set(fan_en,1);
	}
	else
	{
		if(speed>=100) speed=99; //TODO: Fix this bug

		if(speed <= 0)
		{
			TRACE(TRACE_INFO,"zero speed");
			pwm_configure_pin2((const pio_pin_t *)&PIN_mbl_box_fan_en, 0, 1000); //If we put to 1000 it almost get 0
			shw_pio_set(fan_en,0);
		}
		else pwm_configure_pin2((const pio_pin_t *)&PIN_mbl_box_fan_en, 1, (int)(speed*10));
	}
}

const pio_pin_t PIN_mbl_box_led_cpu1[] =  {{PIOA, 0, PIO_MODE_GPIO, PIO_OUT}, {0}};
const pio_pin_t PIN_mbl_box_led_cpu2[] =  {{PIOA, 1, PIO_MODE_GPIO, PIO_OUT}, {0}};

void led_test(int led_id)
{
	int i;
	static unsigned char pattern1[] = {0,1,0,1};
	static unsigned char pattern2[] = {1,0,0,1};

	//Configuration
	TRACE(TRACE_INFO,"Starting led_test tools");
	shw_pio_mmap_init();
	shw_pio_configure((const pio_pin_t *)&PIN_mbl_box_led_cpu1);
	shw_pio_configure((const pio_pin_t *)&PIN_mbl_box_led_cpu2);

	if(led_id<=2)
	{
		//Test pattern
		for(i=0; i<4;i++)
		{
			if(led_id==0 || led_id==1) shw_pio_set((const pio_pin_t *)&PIN_mbl_box_led_cpu1,pattern1[i]);
			if(led_id==0 || led_id==2) shw_pio_set((const pio_pin_t *)&PIN_mbl_box_led_cpu2,pattern2[i]);
			sleep(1);
		}
	}
	else
	{
		TRACE(TRACE_FATAL, "led_id=%d is not supported", led_id);
	}
}



int main(int argc, char **argv)
{

	if (argc < 2) usage_error(argv);
	const char *command = argv[1];

	trace_log_stderr();

	if (strcmp(command, "--fan") == 0) {
		fan_speed(atoi(argv[2]));
	}
	else if (strcmp(command, "--led") == 0) {
		led_test(atoi(argv[2]));
	}
	else if (strcmp(command, "--sfp") == 0) {
		printf("Not implemented");
		usage_error(argv);
	}
	else {
		printf("Command: '%s' is unknown",command);
		usage_error(argv);
	}
	return 0;
}


