#include <stdio.h>
#include <stdlib.h>

#include "robotcar.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "xtimer.h"

#ifndef MOTOR_FREQUENCY
#define MOTOR_FREQUENCY       (1000U)
#endif

#ifndef MOTOR_RESOLUTION
#define MOTOR_RESOLUTION      (1000U)
#endif

int robot_init(robot_t *dev, pwm_t pwm, int pwm_channel, gpio_t in1, gpio_t in2, gpio_t stby, gpio_t encoder, int id)
{
    int actual_frequency;

    actual_frequency = pwm_init(pwm, PWM_LEFT, MOTOR_FREQUENCY, MOTOR_RESOLUTION);

    //printf("motor: requested %d hz, got %d hz\n", MOTOR_FREQUENCY, actual_frequency);

    if (actual_frequency < 0) {
        /* PWM error */
        return -1;
    }

    dev->device = pwm;
    dev->channel = pwm_channel;
    dev->in1 = in1;
    dev->in2 = in2;
    dev->encoder = encoder;
    dev->stby = stby;
    dev->counter = 0;
    dev->id = id;
   
    if(gpio_init(dev->in1,GPIO_OUT) < 0) {return -1;}
    if(gpio_init(dev->in2,GPIO_OUT) < 0){return -1;}
    if(gpio_init(dev->stby,GPIO_OUT) < 0){return -1;}
    if(gpio_init(dev->encoder,GPIO_OUT) < 0){return -1;}

    return 0;
}

void robot_step_counter( robot_t *dev){
 dev->counter++;
 printf("step motor %d: %d \n",dev->id,dev->counter);
}


int cm_to_steps(uint8_t cm){
  
  int result;
  float circumference = (WHEEL_DIAM * 3.14)/10;
  float cm_step = circumference / STEP_COUNT;
  float f_result = cm/cm_step;
  result = (int)f_result;

  return result;
}

void robot_fw( robot_t *dev, uint16_t speed){
	gpio_set(dev->in1);
	gpio_clear(dev->in2);
	pwm_set(dev->device,dev->channel, speed);
}
void robot_rev( robot_t *dev, uint16_t speed){
	gpio_clear(dev->in1);
	gpio_set(dev->in2);
	pwm_set(dev->device,dev->channel,speed);
}

void robot_drive( robot_t *dev, uint16_t speed, int direction){
        gpio_set(dev->stby);
	if(direction < 0) robot_fw(dev,speed);
        else robot_rev(dev,speed);
}

void robot_stop(robot_t *dev1, robot_t *dev2){
        gpio_set(dev1->stby);
        gpio_clear(dev1->in1);
	gpio_clear(dev1->in2);
        gpio_clear(dev2->in1);
	gpio_clear(dev2->in2);
        dev1->counter = 0;
	dev2->counter = 0;
} 

void robot_move_forward( robot_t *dev1,  robot_t *dev2, uint16_t speed,int steps){
	 
	 
        while((steps > dev1->counter)&&(steps > dev2->counter))
	{
	         
	 if(steps > dev1->counter)robot_drive(dev1,speed,1);
	 if(steps > dev2->counter)robot_drive(dev2,speed,1);                 
 
       }
   	robot_stop(dev1,dev2);    
        
}


void robot_move_reverse( robot_t *dev1,  robot_t *dev2, uint16_t speed,int steps){
	
	 
        while((steps > dev1->counter)&&(steps > dev2->counter))
	{
	         
		if(steps > dev1->counter)robot_drive(dev1,speed,-1);
	        if(steps > dev2->counter)robot_drive(dev2,speed,-1);
                 
 
       }
   	robot_stop(dev2,dev1);    
       
}

void robot_spin_right( robot_t *dev1,  robot_t *dev2, uint16_t speed,int steps){
	
	
        while((steps > dev1->counter)&&(steps > dev2->counter))
	{
	         
		if(steps > dev1->counter)robot_drive(dev1,speed,-1);
	        if(steps > dev2->counter)robot_drive(dev2,speed,1);
                 
 
       }
   	robot_stop(dev1,dev2);    
        
}

void robot_spin_left( robot_t *dev1,  robot_t *dev2, uint16_t speed,int steps){
	
	
        while((steps > dev1->counter)&&(steps > dev2->counter))
	{
	         
		if(steps > dev1->counter)robot_drive(dev1,speed,1);
	        if(steps > dev2->counter)robot_drive(dev2,speed,-1);
                 
 
       }
   	robot_stop(dev1,dev2);    
        
}