/*
 * helloworld.c: simple test application
 *
 * This application configures UART 16550 to baud rate 9600.
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

/*
#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"


int main()
{
    init_platform();

    print("Hello World\n\r");

    cleanup_platform();
    return 0;
}
*/

#include "platform.h"
#include "xil_printf.h"
#include "xil_io.h"
#include "xparameters.h"
#include "xgpio.h"


#define PWM_BASE 0x42800000 //XPAR_AXI_TIMER_0_BASEADDR
#define PWM_BASE1 0x42810000 //XPAR_AXI_TIMER_0_BASEADDR


//inches

#define PERIOD        100000    //10kHZ frequency

#define LOW_SPEED     30000     //30% Duty Cycle
#define MEDIUM_SPEED  50000     //50% Duty Cycle
#define HIGH_SPEED    70000     //70% Duty Cycle

#define MOTOR_DIRECTION_CHANNEL 1
#define WHEEL_CIRCUMFERENCE 101
#define ENCODER_CHANNEL        2

#define MOTOR_LEFT_FORWARD        0b00000100
#define MOTOR_LEFT_BACKWARD       0b00001000
#define MOTOR_LEFT_SHORT_BRAKE    0b00001100

#define MOTOR_RIGHT_FORWARD       0b00000001
#define MOTOR_RIGHT_BACKWARD      0b00000010
#define MOTOR_RIGHT_SHORT_BRAKE   0b00000011

#define partstDIRECTION_OUTPUT	( 1 )
#define partstDIRECTION_INPUT	( 0 )

#define STRAIGHT_TIME 250000000
#define TURN_TIME     65000000

static XGpio xGpio;
//BaseType_t xStatus;
XGpio_Config *pxConfigPtr;

u32 encoder_ticks = 0;
uint32_t i;
int motorFun()
{
	int i = 0;
 init_platform();

 // Initialize the AXI GPIO for for the MOTOR Direction Control and the Encoder Tick Count
 pxConfigPtr = XGpio_LookupConfig( XPAR_AXI_GPIO_0_DEVICE_ID );
 //xStatus =
 XGpio_CfgInitialize( &xGpio, pxConfigPtr, pxConfigPtr->BaseAddress);
 //configASSERT( xStatus == XST_SUCCESS );
 //( void ) xStatus; //  Remove compiler warning if configASSERT() is not defined.
 XGpio_SetDataDirection(&xGpio, MOTOR_DIRECTION_CHANNEL,partstDIRECTION_OUTPUT);
 XGpio_SetDataDirection(&xGpio, ENCODER_CHANNEL,partstDIRECTION_INPUT);



 /* Set the period and duty cycle of the PWM signal.
 * The first call sets TLR0 to have a period of 20ms.
 * The second call sets TLR0 to have a duty cycle of 50% (10ms period).
 * The formula to calculate the value to use is
 * (period / clk_period) - 2
 * The values are in nanoseconds
 * The clk_period is the period of the AXI Timer aclk input,
 * which is 100MHz (10ns period) (the default from FCLK).
 */

 /*
 Xil_Out32(PWM_BASE + 0x4, (20e6 / 10) - 2);
 Xil_Out32(PWM_BASE + 0x14, (10e6 / 10) - 2);

 //20%duty cycle
 Xil_Out32(PWM_BASE1 + 0x4, (20e6 / 10) - 2);
 Xil_Out32(PWM_BASE1 + 0x14, (2e6 / 10) - 2);
*/

//LEFT MOTOR
 Xil_Out32(PWM_BASE + 0x4, (PERIOD / 10) - 2);
 Xil_Out32(PWM_BASE + 0x14, (20000 / 10) - 2);

//RIGHT MOTOR
 Xil_Out32(PWM_BASE1 + 0x4, (PERIOD / 10) - 2);
 Xil_Out32(PWM_BASE1 + 0x14, (20000 / 10) - 2);


 /* Write our control values to the TCSRx registers.
 * In both, the PWMAx bit is set to enable PWM and
 * UDTx is set to put the timer in down count mode.
 * The GENTx bit is also set as required for PWM mode.
 * In the second write, we write also enable both timers
 */


 Xil_Out32(PWM_BASE, 0x206);
 Xil_Out32(PWM_BASE + 0x10, 0x606);
 Xil_Out32(PWM_BASE1, 0x206);
 Xil_Out32(PWM_BASE1 + 0x10, 0x606);
 //printf("Initialization done...\n");

//

 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_FORWARD | MOTOR_LEFT_FORWARD);

 while (1) {
	 i = 0;

	 /* Do nothing */
	 //encoder_ticks = XGpio_DiscreteRead(&xGpio, ENCODER_CHANNEL);
	 //make the motors spin forward
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_FORWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<STRAIGHT_TIME;i++); //straight
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_BACKWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<TURN_TIME;i++); //turn right
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_FORWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<STRAIGHT_TIME;i++); //straight
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_BACKWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<TURN_TIME;i++); //turn right
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_FORWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<STRAIGHT_TIME;i++); //straight
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_BACKWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<TURN_TIME;i++);
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_FORWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<STRAIGHT_TIME;i++); //
	 ++i;
	 XGpio_DiscreteWrite(&xGpio, MOTOR_DIRECTION_CHANNEL, MOTOR_RIGHT_BACKWARD | MOTOR_LEFT_FORWARD);
	 for (i=0;i<TURN_TIME;i++); //
 }
 cleanup_platform();
 return 0;
}