/**
 * Keil project example for I2C - Multiple write and read bytes from SLAVE
 *
 * Before you start, select your target, on the right of the "Load" button
 *
 * @author    Tilen Majerle
 * @email     tilen@majerle.eu
 * @website   http://stm32f4-discovery.com
 * @ide       Keil uVision 5
 * @conf      PLL parameters are set in "Options for Target" -> "C/C++" -> "Defines"
 * @packs     STM32F4xx/STM32F7xx Keil packs are requred with HAL driver support
 * @stdperiph STM32F4xx/STM32F7xx HAL drivers required
 */
/* Include core modules */
#include "stm32fxxx_hal.h"
/* Include my libraries here */
#include "defines.h"
#include "tm_stm32_disco.h"
#include "tm_stm32_delay.h"
#include "tm_stm32_i2c.h"
#include <string.h>

/* PoStep60 device address */
#define POSTEP60_ADDRESS     0x4d

/* Byte value read from external device */
uint8_t data_array[6];

int main(void) {

	int i;
	int32_t pos;

	/* Init system clock for maximum system speed */
	TM_RCC_InitSystem();

	/* Init HAL layer */
	HAL_Init();

	/* Init leds */
	TM_DISCO_LedInit();

	/* Init delay */
	TM_DELAY_Init();

	/* Init I2C, SCL = PB8, SDA = PB9, available on Arduino headers and on all discovery boards */
	/* For STM32F4xx and STM32F7xx lines */
	TM_I2C_Init(I2C3, TM_I2C_PinsPack_1, 100000);

		/* Format data array to write to device */
	data_array[0] = 0x01; /* Command = 0x01 */
	data_array[1] = 0x11; /* Data 1 */
	data_array[2] = 0x22; /* Data 2 */
	data_array[3] = 0x33; /* Data 3 */
	data_array[4] = 0xcc; /* Data 4 */

	// Test loopback
	TM_I2C_WriteMultiNoRegister(I2C3, POSTEP60_ADDRESS, data_array, 5);
	HAL_Delay(1);
	for (i = 0; i<5; i++) data_array[i]=0;
	TM_I2C_ReadMultiNoRegister(I2C3, POSTEP60_ADDRESS, data_array, 5);
	HAL_Delay(1);


	// Run
	data_array[0] = 0x03; /* Command 0x03 – Set Run/Sleep mode  */
	data_array[1] = 0xda; /* Run 0xDA */
	TM_I2C_WriteMultiNoRegister(I2C3, POSTEP60_ADDRESS, data_array, 2);
	HAL_Delay(1);

	pos = 0;


	// Set zero
	data_array[0] = 0x5e; /* Command 0x5E – Set zero  */
	TM_I2C_WriteMultiNoRegister(I2C3, POSTEP60_ADDRESS, data_array, 1);
	HAL_Delay(1);


  while (1) {

		// Set position
		data_array[0] = 0x50; /* Command 0x50 – Set position  */
		memcpy(&data_array[1], &pos, 4);
		TM_I2C_WriteMultiNoRegister(I2C3, POSTEP60_ADDRESS, data_array, 5);
		HAL_Delay(2000);

		if (pos>0) pos = -5000; else pos=5000;

	}
}