#include "openEL.h"
#include "openEL_motor.h"
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

uint32_t now1 = 0;
uint32_t now2 = 0;

void position1(uint32_t m, uint32_t pos)
{
	int i;
	int32_t d = now1 - pos;

	for (i = 0; i < abs(d) / 5 - 1; ++i) {
		if (0 <= d)
			HalMotorSetPosition_I32(m, now1 - 5 * i, 0, 0);
		else
			HalMotorSetPosition_I32(m, now1 + 5 * i, 0, 0);
		usleep(40 * 1000);
	}
	HalMotorSetPosition_I32(m, pos, 0, 0);
	now1 = pos;
}

void position2(uint32_t m, uint32_t pos, uint32_t *now)
{
	int i;
	int32_t d = (int32_t)*now - (int32_t)pos;

	for (i = 0; i < abs(d) / 5 - 1; ++i) {
		if (0 <= d)
			HalMotorSetPosition_I32(m, *now - 5 * i, 0, 0);
		else
			HalMotorSetPosition_I32(m, *now + 5 * i, 0, 0);
		usleep(40 * 1000);
	}
	HalMotorSetPosition_I32(m, pos, 0, 0);
	*now = pos;
}

#define WAIT 8

int main()
{
	uint32_t count = 0;
	uint32_t motor1 = 0;
	uint32_t motor2 = 0;
	uint32_t *Motor_1;
	uint32_t *Motor_2;
	int rnd;
	int rnd_old = 0;
	int off = 45;

	Motor_1 = &motor1;
	Motor_2 = &motor2;

	HalInit(Motor_1, 0, 0, 16);

	printf("deg 90\n");
	HalMotorSetPosition_I32(*Motor_1, 90 + off, 0, 0);
	now1 = 90;
	sleep(WAIT);

	for (;;) {
		rnd = rand() % 7;
		if (rnd_old == rnd)
			continue;
		rnd_old = rnd;

		if (10 < count++) {
			rnd = 3;
			count = 0;
		}

//		printf("rnd %d\n", rnd);

		if (rnd == 0) {
			printf("deg 45\n");
			position2(*Motor_1, 45 + off, &now1);
			sleep(WAIT);
		} else if (rnd == 1) {
			printf("deg 60\n");
			position2(*Motor_1, 60 + off, &now1);
			sleep(WAIT);
		} else if (rnd == 2) {
			printf("deg 75\n");
			position2(*Motor_1, 75 + off, &now1);
			sleep(WAIT);
		} else if (rnd == 3) {
			printf("deg 90\n");
			position2(*Motor_1, 90 + off, &now1);
			sleep(WAIT);
		} else if (rnd == 4) {
			printf("deg 105\n");
			position2(*Motor_1, 105 + off, &now1);
			sleep(WAIT);
		} else if (rnd == 5) {
			printf("deg 120\n");
			position2(*Motor_1, 120 + off, &now1);
			sleep(WAIT);
		} else if (rnd == 6) {
			printf("deg 135\n");
			position2(*Motor_1, 135 + off, &now1);
			sleep(WAIT);
		}
	}

	HalMotorSetPosition_I32(*Motor_1, 90 + off, 0, 0);
	HalExit(*Motor_1);
}
