/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
// Include libraries
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
#include "communication.h"
#include <stdio.h>

// Define pins
#define MOTEUR1_PIN_SENS1 4
#define MOTEUR1_PIN_SENS2 5
#define MOTEUR1_PIN_ACTIVATION 0
#define MOTEUR2_PIN_SENS1 6
#define MOTEUR2_PIN_SENS2 7
#define MOTEUR2_PIN_ACTIVATION 1
#define MOTEUR3_PIN_SENS1 8
#define MOTEUR3_PIN_SENS2 9
#define MOTEUR3_PIN_ACTIVATION 2
#define MOTEUR4_PIN_SENS1 10
#define MOTEUR4_PIN_SENS2 11
#define MOTEUR4_PIN_ACTIVATION 3

// Juste pour information - les broches sont re-définies dans i2c_slave
#define I2C0_SDA_PIN 16
#define I2C0_SCL_PIN 17

// Juste pour information - les broches sont re-définies dans i2c_master
#define I2C1_SDA_PIN 18
#define I2C1_SCL_PIN 19

// Init all motion motors pins
void Init_motion_motor(void);
// Set motor 1 speed forward
void Motor1_forward(int speed);
// Set motor 1 speed backward
void Motor1_backward(int speed);
// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void Motor1_speed(int speed);
// Set motor 2 speed forward
void Motor2_forward(int speed);
// Set motor 2 speed backward
void Motor2_backward(int speed);
// Set motor 2 speed and direction (negative value : backward / positive value : forward)
void Motor2_speed(int speed);
// Set motor 3 speed forward
void Motor3_forward(int speed);
// Set motor 3 speed backward
void Motor3_backward(int speed);
// Set motor 3 speed and direction (negative value : backward / positive value : forward)
void Motor3_speed(int speed);
// Set motor 1 speed forward
void Motor2_forward(int speed);
// Set motor 1 speed backward
void Motor3_backward(int speed);
// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void Motor4_speed(int speed);
// Convert te joystick value to an char value
uint8_t Conversions_joystick2uint8(uint16_t joystick_value);

void main(void)
{
	char message[256], reception[256];
    stdio_init_all();

	Init_motion_motor();
	communication_init();


	// Exemple de communication
	message[0]='B';
	message[1]='a';
	message[2]='\n';
	message[3]='\0';
	
	while(1){

		printf("Envoi message\n");
		communication_send_message(message, 4);

		printf("Lire message\n");
		if (communication_read_message(reception) == I2C_ECHEC){
			printf("Echec de la lecture du message\n");
		}else{
			printf("Succes\n");
			printf("%s",reception);
		}
		
		sleep_ms(1000);
	}



	while(1)
	{
		Motor1_speed(-1000);
		Motor2_speed(-1000);
		Motor3_speed(-1000);
		Motor4_speed(-1000);
		sleep_ms(3000);
		Motor1_speed(1000);
		Motor2_speed(1000);
		Motor3_speed(1000);
		Motor4_speed(1000);
		sleep_ms(3000);
	}

}

// Init all motion motors pins
void Init_motion_motor(void)
{
	// Init 1/0 pin for control motion motors
	gpio_init(MOTEUR1_PIN_SENS1);
	gpio_init(MOTEUR1_PIN_SENS2);
	gpio_init(MOTEUR2_PIN_SENS1);
	gpio_init(MOTEUR2_PIN_SENS2);
	gpio_init(MOTEUR3_PIN_SENS1);
	gpio_init(MOTEUR3_PIN_SENS2);
	gpio_init(MOTEUR4_PIN_SENS1);
	gpio_init(MOTEUR4_PIN_SENS2);
	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR3_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR3_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR4_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR4_PIN_SENS2, GPIO_OUT);

	// Set direction to 0 (disactivate)
	gpio_put(MOTEUR1_PIN_SENS1, 0);
	gpio_put(MOTEUR1_PIN_SENS2, 0);
	gpio_put(MOTEUR2_PIN_SENS1, 0);
	gpio_put(MOTEUR2_PIN_SENS2, 0);
	gpio_put(MOTEUR3_PIN_SENS1, 0);
	gpio_put(MOTEUR3_PIN_SENS2, 0);
	gpio_put(MOTEUR4_PIN_SENS1, 0);
	gpio_put(MOTEUR4_PIN_SENS2, 0);

	// Init pwm pins for motion motors
	gpio_init(MOTEUR1_PIN_ACTIVATION);
	gpio_init(MOTEUR2_PIN_ACTIVATION);
	gpio_init(MOTEUR3_PIN_ACTIVATION);
	gpio_init(MOTEUR4_PIN_ACTIVATION);
	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR3_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR4_PIN_ACTIVATION, GPIO_FUNC_PWM);

	// Set wrap of pwm slices
	pwm_set_wrap(0, 1000);
	pwm_set_wrap(1, 1000);

	// Active all pwm slices
	pwm_set_enabled(0, true);
	pwm_set_enabled(1, true);

	// Set speed to 0
	pwm_set_chan_level(0, PWM_CHAN_A, 0);
	pwm_set_chan_level(0, PWM_CHAN_B, 0);
	pwm_set_chan_level(1, PWM_CHAN_A, 0);
	pwm_set_chan_level(1, PWM_CHAN_B, 0);

}

// Set motor 1 speed forward
void Motor1_forward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_A, speed);
	gpio_put(MOTEUR1_PIN_SENS1, 0);
	gpio_put(MOTEUR1_PIN_SENS2, 1);
}

// Set motor 1 speed backward
void Motor1_backward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_A, speed);
	gpio_put(MOTEUR1_PIN_SENS1, 1);
	gpio_put(MOTEUR1_PIN_SENS2, 0);
}

// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void Motor1_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor1_backward(speed);
	}
	else
	{
		Motor1_forward(speed);
	}
}

// Set motor 2 speed forward
void Motor2_forward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_B, speed);
	gpio_put(MOTEUR2_PIN_SENS1, 0);
	gpio_put(MOTEUR2_PIN_SENS2, 1);
}

// Set motor 2 speed backward
void Motor2_backward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_B, speed);
	gpio_put(MOTEUR2_PIN_SENS1, 1);
	gpio_put(MOTEUR2_PIN_SENS2, 0);
}

// Set motor 2 speed and direction (negative value : backward / positive value : forward)
void Motor2_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor2_backward(speed);
	}
	else
	{
		Motor2_forward(speed);
	}
}

// Set motor 3 speed forward
void Motor3_forward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_A, speed);
	gpio_put(MOTEUR3_PIN_SENS1, 0);
	gpio_put(MOTEUR3_PIN_SENS2, 1);
}

// Set motor 3 speed backward
void Motor3_backward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_A, speed);
	gpio_put(MOTEUR3_PIN_SENS1, 1);
	gpio_put(MOTEUR3_PIN_SENS2, 0);
}

// Set motor 3 speed and direction (negative value : backward / positive value : forward)
void Motor3_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor3_backward(speed);
	}
	else
	{
		Motor3_forward(speed);
	}
}

// Set motor 4 speed forward
void Motor4_forward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_B, speed);
	gpio_put(MOTEUR4_PIN_SENS1, 0);
	gpio_put(MOTEUR4_PIN_SENS2, 1);
}

// Set motor 4 speed backward
void Motor4_backward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_B, speed);
	gpio_put(MOTEUR4_PIN_SENS1, 1);
	gpio_put(MOTEUR4_PIN_SENS2, 0);
}

// Set motor 4 speed and direction (negative value : backward / positive value : forward)
void Motor4_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor4_backward(speed);
	}
	else
	{
		Motor4_forward(speed);
	}
}

/// @brief Prend un nombre entre 0 et 4096, et sort un nombre entre 0 et 255
/// @return un nombre entre 0 et 255
uint8_t Conversions_joystick2uint8(uint16_t joystick_value)
{
	uint8_t valeur = joystick_value / 4;
	return valeur;
}