/**
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/uart.h"
#include "hardware/irq.h"

//#define USEN
#define PWM_ON

#define UART_ID uart1
#define BAUD_RATE 9600
#define DATA_BITS 8
#define STOP_BITS 1
#define PARITY    UART_PARITY_NONE


#define ON 1
#define OFF 0
#define UART_TX_PIN 8
#define UART_RX_PIN 9
const int  ON_BORD_LED = 25;

// Define the Pins
const int L_FIN = 0 ;
const int L_RIN = 2;
const int R_FIN = 4;
const int R_RIN = 6;
//SLIDE_POT =  26
const int FRONT_BUTTON_Pin = 21;
const int LEFT_BUTTON_Pin = 20;
const int RIGHT_BUTTON_Pin = 19;
const int BACK_BUTTON_Pin = 18;

const int DIV_CLK = 50.0;
const int SYS_CLK = 125000000;  //125MHz
const int PWM_FREQ = 50000;  //range:20kHz~100kHz
const int PWM_CYCLE = 1/PWM_FREQ*1000000;  //us

//グルーバル変数の宣言
static int chars_rxed = 0;
static int data_num=0;
uint8_t sbus_data[1];
uint8_t ch;
static pwm_config pwm_slice_config_l, pwm_slice_config_r;
uint pwm0_slice_num_l, pwm0_slice_num_r;
//uint16_t Chdata[6];

//int duty_l, duty_r;
int duty;
//int duty_rev;

//関数の宣言
uint8_t init_serial(void);
uint8_t init_pwm();
uint8_t init_gpio();
void on_uart_rx(); 

uint8_t init_serial(void){

    /// シリアル通信の設定

    // UARTを基本の通信速度で設定
    uart_init(UART_ID, 2400);

    // 指定のGPIOをUARTのTX、RXピンに設定する
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);

    //指定のUARTを指定の通信速度に設定する
    int actual = uart_set_baudrate(UART_ID, BAUD_RATE);

    //UART flow control CTS/RTSを使用しない設定
    uart_set_hw_flow(UART_ID, false, false);

    //通信フォーマットの設定
    uart_set_format(UART_ID, DATA_BITS, STOP_BITS, PARITY);

    // Turn off FIFO's - we want to do this character by character
    uart_set_fifo_enabled(UART_ID, false);

    // Set up a RX interrupt
    // We need to set up the handler first
    // Select correct interrupt for the UART we are using
    int UART_IRQ = UART_ID == uart0 ? UART0_IRQ : UART1_IRQ;

    // And set up and enable the interrupt handlers
    irq_set_exclusive_handler(UART_IRQ, on_uart_rx);
    irq_set_enabled(UART_IRQ, true);

    // Now enable the UART to send interrupts - RX only
    uart_set_irq_enables(UART_ID, true, false);
}

uint8_t init_pwm(){

   // PWMの設定
   // Tell GPIO 0 and 1 they are allocated to the PWM
	gpio_set_function( L_FIN, GPIO_FUNC_PWM );
	gpio_set_function( R_FIN, GPIO_FUNC_PWM );


    // Find out which PWM slice is connected to GPIO 0 (it's slice 0)
	pwm0_slice_num_l = pwm_gpio_to_slice_num( L_FIN );
	pwm0_slice_num_r = pwm_gpio_to_slice_num( R_FIN );

	//PWM wrap計算
	uint pwm_wrap;
	//pwm_wrap = SYS_CLK/(PWM_FREQ*DIV_CLK)-1;
	pwm_wrap = SYS_CLK/(PWM_FREQ*DIV_CLK*2)-1;  //位相補正ONのとき、周期が2倍になるため
	
	//PWMコンフィグdefault値取得
	pwm_slice_config_l = pwm_get_default_config();
	pwm_slice_config_r = pwm_get_default_config();
	//PWM周波数設定
	pwm_config_set_clkdiv( &pwm_slice_config_l,DIV_CLK );
	pwm_config_set_clkdiv( &pwm_slice_config_r,DIV_CLK );
	pwm_config_set_wrap( &pwm_slice_config_l,pwm_wrap );
	pwm_config_set_wrap( &pwm_slice_config_r,pwm_wrap );
	//PWM位相補正ON
	pwm_config_set_phase_correct (&pwm_slice_config_l,ON);
	pwm_config_set_phase_correct (&pwm_slice_config_r,ON);
	//PWMコンフィグ値反映
	pwm_init( pwm0_slice_num_l, &pwm_slice_config_l, false );
	pwm_init( pwm0_slice_num_r, &pwm_slice_config_r, false );
	//デューティー比設定	
	//duty = 50;
	duty = 100;  //PWM出力初期値'H'
	//pwm_set_gpio_level( L_FIN, ( pwm_slice_config_l.top * duty/100 ) );
	//pwm_set_gpio_level( R_FIN, ( pwm_slice_config_r.top * duty/100 ) );
	pwm_set_chan_level( pwm0_slice_num_l, PWM_CHAN_A, ( pwm_slice_config_l.top * duty/100 ));//*0.5 );//位相補正ONのとき、周期が2倍になるため
	pwm_set_chan_level( pwm0_slice_num_r, PWM_CHAN_A, ( pwm_slice_config_r.top * duty/100 ));//*0.5 );//位相補正ONのとき、周期が2倍になるため

	pwm_set_enabled (pwm0_slice_num_l, 1);
	pwm_set_enabled (pwm0_slice_num_r, 1);
	
	sleep_us(10);
	
	pwm_set_enabled (pwm0_slice_num_l, 0);
	pwm_set_enabled (pwm0_slice_num_r, 0);

//    // Set channel A output high for one cycle before dropping
//    pwm_set_chan_level(slice_num, PWM_CHAN_A, 2315);
//    // Set initial B output high for three cycles before dropping
//    pwm_set_chan_level(slice_num, PWM_CHAN_B, 1330);
//    // Set the PWM running
//    pwm_set_enabled(slice_num, true);
//    /// \end::setup_pwm[]
    sleep_ms(200);
//    pwm_set_chan_level(slice_num, PWM_CHAN_A, 1330);
}

uint8_t init_gpio(void){
	
	//ボタン入力GPIOを入力に設定しpullup
	gpio_init( FRONT_BUTTON_Pin );
    gpio_init( LEFT_BUTTON_Pin );
    gpio_init( RIGHT_BUTTON_Pin );
    gpio_init( BACK_BUTTON_Pin );
	
	gpio_set_dir( FRONT_BUTTON_Pin,	 GPIO_IN );
	gpio_set_dir( LEFT_BUTTON_Pin,	 GPIO_IN );
	gpio_set_dir( RIGHT_BUTTON_Pin,	 GPIO_IN );
	gpio_set_dir( BACK_BUTTON_Pin,	 GPIO_IN );
	
	gpio_pull_up( FRONT_BUTTON_Pin );
	gpio_pull_up( LEFT_BUTTON_Pin );
	gpio_pull_up( RIGHT_BUTTON_Pin );
	gpio_pull_up( BACK_BUTTON_Pin );
		
	//PWM出力GPIO設定
	//初期化
//    gpio_init(L_FIN);
    gpio_init(L_RIN);
//    gpio_init(R_FIN);
    gpio_init(R_RIN);
    
	//gpio set dir	
    gpio_set_dir(L_RIN,				GPIO_OUT);
    gpio_set_dir(R_RIN,				GPIO_OUT);
	
	gpio_put(L_RIN,1);
	gpio_put(R_RIN,1);
	
	return 0;
}

// RX interrupt handler
void on_uart_rx() {
    short data;
    while (uart_is_readable(UART_ID)) {
        ch = uart_getc(UART_ID);
        //sbus_data[chars_rxed]=ch;
		//Chdata[0] = sbus_data[chars_rxed];
                
    }
}


int main(){
	stdio_init_all();
	printf("config start\n");
	//LED GPIO設定
	gpio_init(ON_BORD_LED);
	gpio_set_dir(ON_BORD_LED,GPIO_OUT);
	gpio_put(ON_BORD_LED, 1);
	sleep_ms(750);

	init_gpio();
	init_serial();
	init_pwm();
	
	gpio_put( L_RIN, 1);
	gpio_put( L_RIN, 1);
	
	//PWM ENABLE
	uint pwm_mask_enabled=0x0;
	pwm_mask_enabled = ( 0x01 << pwm0_slice_num_l ) | ( 0x01 << pwm0_slice_num_r );
    pwm_set_mask_enabled( pwm_mask_enabled );

	uint8_t dir = 0;
	uint8_t dir0 = 0;
	uint8_t pwm = 1;
	ch = 0x10;
	
	gpio_put(ON_BORD_LED, 0);
    sleep_ms(250);
	duty = 50;
	//duty_rev = 50;
	
	
	uint8_t duty_l, duty_r;
	uint8_t l_rin_out, r_rin_out;
	uint8_t dbg_pin0=1;
	uint8_t dbg_pin1=1;
	
	
    while(1){
		tight_loop_contents();
		dir0 = ch & 0xF;
		pwm = (ch & 0x30)>>4;		
//		printf("uart in=%04x, dir(uart[3:0])=%04x, pwm(uart[5:4])=%04x\n",ch,dir0,pwm);
		
#ifdef USEN
		dir = ((!gpio_get(BACK_BUTTON_Pin))<<3 )|((!gpio_get(RIGHT_BUTTON_Pin))<<2)|((!gpio_get(LEFT_BUTTON_Pin))<<1)|(!gpio_get(FRONT_BUTTON_Pin));
#else
		dir = ch & 0xF;
		pwm = (ch & 0x30)>>4;
#ifdef PWM_ON
		switch(pwm){
			case 0:	duty = 25; 	break;
			case 1:	duty = 40; 	break;
			case 2:	duty = 50; 	break;
			case 3:	duty = 75; break;
		}
		//duty_rev = 100 - duty;
#endif
#endif

		//printf("uart in=%04x, dir(uart[3:0])=%04x, pwm(uart[5:4])=%04x, duty = %d\n",ch,dir0,pwm,duty);

		
		switch(dir){
//			case 0:
			case 1:
				//printf("front, ");
				duty_l = duty;
				duty_r = duty;
				l_rin_out = 0;
				r_rin_out = 0;
				break;
			case 2:
				//printf("left, ");
				duty_l = 100;   //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				duty_r = duty;
				l_rin_out = 1;  //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				r_rin_out = 0;
				break;
			case 4:
				//printf("rigth, ");
				duty_l = duty;
				duty_r = 100;   //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				l_rin_out = 0;
				r_rin_out = 1;  //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				break;
			case 8:
				//printf("back, ");
				duty_l = 100 - duty;
				duty_r = 100 - duty;
				l_rin_out = 1;
				r_rin_out = 1;
				break;
			case 3:
				//printf("left front, ");
				duty_l = duty*0.5;
				duty_r = duty;
				l_rin_out = 0;
				r_rin_out = 0;
				break;
			case 5:
				//printf("right front, ");
				duty_l = duty;
				duty_r = duty*0.5;
				l_rin_out = 0;
				r_rin_out = 0;
				break;
			case 10:
				//printf("left back, ");
				duty_l = 100 - duty*0.5;
				duty_r = 100 - duty;
				l_rin_out = 1;
				r_rin_out = 1;
				break;
			case 12:
				//printf("right back, ");
				duty_l = 100 - duty;
				duty_r = 100 - duty*0.5;
				l_rin_out = 1;
				r_rin_out = 1;
				break;
			case 6:
				//printf("right yaw, ");
				duty = 25;
				duty_l = duty;
				duty_r = 100 - duty;
				l_rin_out = 0;
				r_rin_out = 1;
				break;
			case 9:
				//printf("left yaw, ");
				duty = 25;
				duty_l = 100 - duty;
				duty_r = duty;
				l_rin_out = 1;
				r_rin_out = 0;
				break;			
			default:
				//printf("stop, ");
				duty_l = 100;   //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				duty_r = 100;   //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				l_rin_out = 1;  //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				r_rin_out = 1;  //0; 停止時のモータードライバのmode設定をd(ブレーキ：FIN=H,RIN=Hに変更)
				break;

		}
		pwm_set_chan_level( pwm0_slice_num_l, PWM_CHAN_A, ( pwm_slice_config_l.top * duty_l/100 ) );
		pwm_set_chan_level( pwm0_slice_num_r, PWM_CHAN_A, ( pwm_slice_config_r.top * duty_r/100 ) );
		dbg_pin0 = gpio_get(L_FIN);
		dbg_pin1 = gpio_get(R_FIN);
		//printf("duty_r=%d, duty_l=%d, pwm_mask_enable=%x, l_rin_out=%d, r_rin_out=%d, dbg_pin_LFIN=%d, dbg_pin_RFIN=%d\n",duty_r,duty_l,pwm_mask_enabled,l_rin_out,r_rin_out,dbg_pin0,dbg_pin1);
		printf("dir=%04x, pwm=%04x, duty_r=%d, duty_l=%d, pwm_mask_enable=%x, l_rin_out=%d, r_rin_out=%d, dbg_pin_LFIN=%d, dbg_pin_RFIN=%d\n",dir,pwm,duty_r,duty_l,pwm_mask_enabled,l_rin_out,r_rin_out,dbg_pin0,dbg_pin1);
		
		//sleep_ms(1);  //PWM設定の反映待ち(主に停止動作のため)
		sleep_us(PWM_CYCLE*2);  //PWM2周期分wait
		
		//pwm_set_mask_enabled( pwm_mask_enabled );  //PWM disable時動作が安定しないため、FINの'H''L'はduty=100 or 0で実現する=PWMは常にON
		
		gpio_put( L_RIN, l_rin_out);
		gpio_put( R_RIN, r_rin_out);


		//sleep_us(1);
		sleep_ms(5);  //uartの通信間隔と調整が必要？
    }

    return 0;
}

//void set_moter(uint pwm_slice_num, uint PWM_CHAN, pwm_config *pwm_slice_config, uint duty, int RIN, uint8_t rin_out){
//	pwm_set_chan_level( pwm_slice_num, PWM_CHAN, ( pwm_slice_config.top * duty/100 ) );
//	gpio_put( RIN, rin_out);
//}