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

#define PWM0_BASE XPAR_AXI_PWM_0_BASEADDR
#define PWM1_BASE XPAR_AXI_PWM_1_BASEADDR
#define PWM2_BASE XPAR_AXI_PWM_2_BASEADDR
#define PWM3_BASE XPAR_AXI_PWM_3_BASEADDR
#define DRIVER_BASE XPAR_MOTOR_DRIVER_AXI_IO_BASEADDR

#define MOTOR0 0x0
#define MOTOR1 0x1
#define MOTOR2 0x2
#define MOTOR3 0x3

#define PWM_FREQ 1e6
#define CLK_FREQ 10

#define GPIO_CHANNEL1 1

static unsigned char current_motor_out = 0x00;

static XGpio encoder0;
static XGpio encoder1;
static XGpio encoder2;
static XGpio encoder3;

void SetMotorOut(unsigned char bit, unsigned char motor_num) {
    /* Sets the High/Low bit for direction*/
    if (bit) {
        current_motor_out = current_motor_out | (1 << motor_num);
    }
    else {
        current_motor_out = current_motor_out & ~(1 << motor_num);
    }
    Xil_Out32(DRIVER_BASE, current_motor_out);
}

void SetPWMDutyCycle(int duty_cycle, unsigned int pwm_base) {
    /*Sets the PWM to a given duty cycle*/
    if (duty_cycle >= 100) {
        Xil_Out32(pwm_base + 0x14, (PWM_FREQ / CLK_FREQ) - 2);
        return;
    }
    int freq = (PWM_FREQ * duty_cycle)/100;
    if (!freq) {
        Xil_Out32(pwm_base + 0x14, 0);
        return;
    }
    Xil_Out32(pwm_base + 0x14, (freq / CLK_FREQ) - 2);
}

void DriveMotor(int power_level, unsigned char motor_num) {
    /*Drives the motor at a given power level*/
    unsigned char motor_out = 0x00;
    if (power_level < 0) {
        motor_out = 0x01;
        power_level += 100;
    }
    SetMotorOut(motor_out, motor_num);
    unsigned int pwm_base;
    switch(motor_num) {
        case MOTOR0:
            pwm_base = PWM0_BASE;
            break;
        case MOTOR1:
            pwm_base = PWM1_BASE;
            break;
        case MOTOR2:
            pwm_base = PWM2_BASE;
            break;
        case MOTOR3:
            pwm_base = PWM3_BASE;
            break;
    }
    SetPWMDutyCycle(power_level, pwm_base);
}

void init_PWMs() {
    /*Initializes the 4 AXI timers used as PWMs*/
    Xil_Out32(PWM0_BASE + 0x4, (PWM_FREQ / CLK_FREQ) - 2);
    Xil_Out32(PWM0_BASE, 0x206);
    Xil_Out32(PWM0_BASE + 0x10, 0x606);

    Xil_Out32(PWM1_BASE + 0x4, (PWM_FREQ / CLK_FREQ) - 2);
    Xil_Out32(PWM1_BASE, 0x206);
    Xil_Out32(PWM1_BASE + 0x10, 0x606);

    Xil_Out32(PWM2_BASE + 0x4, (PWM_FREQ / CLK_FREQ) - 2);
    Xil_Out32(PWM2_BASE, 0x206);
    Xil_Out32(PWM2_BASE + 0x10, 0x606);

    Xil_Out32(PWM3_BASE + 0x4, (PWM_FREQ / CLK_FREQ) - 2);
    Xil_Out32(PWM3_BASE, 0x206);
    Xil_Out32(PWM3_BASE + 0x10, 0x606);
}

void init_encoders() {
    XGpio_Initialize(&encoder0, XPAR_ENCODER_AXI_IO_0_DEVICE_ID);
    XGpio_Initialize(&encoder1, XPAR_ENCODER_AXI_IO_1_DEVICE_ID);
    XGpio_Initialize(&encoder2, XPAR_ENCODER_AXI_IO_2_DEVICE_ID);
    XGpio_Initialize(&encoder3, XPAR_ENCODER_AXI_IO_3_DEVICE_ID);
}

int32_t GetEncoderValue(int motor_num) {
    switch (motor_num) {
        case MOTOR0:
            return XGpio_DiscreteRead(&encoder0, GPIO_CHANNEL1);
        case MOTOR1:
            return XGpio_DiscreteRead(&encoder1, GPIO_CHANNEL1);
        case MOTOR2:
            return XGpio_DiscreteRead(&encoder2, GPIO_CHANNEL1);
        case MOTOR3:
            return XGpio_DiscreteRead(&encoder3, GPIO_CHANNEL1);
        default:
            return 0;
    }
}