#include "common.h"
#include "modules_common.h"
#include "ship0302_control.h"
#include "gpio_common.h"

/*
 * 软 PWM 频率（wiringPi / wiringOP 的 softPwm.c）：
 *   每档时间固定 delayMicroseconds(n*100)，整周期 = pwmRange×100µs
 *   → f = 1/(pwmRange×100µs) = 10000/pwmRange Hz
 * 要约 350Hz：pwmRange ≈ 10000/350 ≈ 28.6，取 29 → 约 344.8Hz（取 28 → 约 357Hz）。
 * 与 gpio_init 里 init_gpiopwm 的 range=100（约 100Hz）不同，此处单独为 ship0302 定档。
 */
#define SHIP0302_SOFT_PWM_MIN 0

/* ship0302 专用软 PWM 引脚（WiringPi），仅在本文件初始化 */
static const int s_ship0302_soft_pwm_pins[] = { 11, 12, 14, 17, 20, -1 };

#define SHIP0302_CTRL_NONE 0
#define SHIP0302_CTRL_PHONE 1
#define SHIP0302_CTRL_STEERING 2

static pthread_mutex_t s_ship0302_hw_mutex = PTHREAD_MUTEX_INITIALIZER;
static int s_ship0302_control_source = SHIP0302_CTRL_NONE;

/** 百分比 0～100 → softPwm 档位 0～SHIP0302_SOFT_PWM_RANGE；100% 即满档，效果上等同输出恒为高 */
static int ship0302_duty_from_percent(int pct_0_100) {
	if (pct_0_100 < 0) {
		pct_0_100 = 0;
	} else if (pct_0_100 > 100) {
		pct_0_100 = 100;
	}
	return (pct_0_100 * SHIP0302_SOFT_PWM_RANGE + 50) / 100;
}

void device_ship0302_init(void) {
	physics_pwm_init();

	for (int i = 0; s_ship0302_soft_pwm_pins[i] != -1; i++) {
		int pin = s_ship0302_soft_pwm_pins[i];
		softPwmCreate(pin, SHIP0302_SOFT_PWM_MIN, SHIP0302_SOFT_PWM_RANGE);
	}
	pinMode(18, OUTPUT);
        digitalWrite(18, LOW); 
	
}

void ship0302_stop(void) {
	pwmWrite(PWM_PIN_SPEED, 0);
	pwmWrite(PWM_PIN_CHANGE, 0);
	for (int i = 0; s_ship0302_soft_pwm_pins[i] != -1; i++) {
		softPwmWrite(s_ship0302_soft_pwm_pins[i], 0);
	}
}

void ship0302_mode_lift_flont(int gval) {
	
	softPwmWrite(12, ship0302_duty_from_percent(100));
	softPwmWrite(14, ship0302_duty_from_percent(5));

}

void ship0302_mode_right_flont(int gval) {
	softPwmWrite(20, ship0302_duty_from_percent(100));
	softPwmWrite(11, ship0302_duty_from_percent(5));

}

void ship0302_mode_lift_back(int gval) {

	softPwmWrite(12, ship0302_duty_from_percent(0));
	softPwmWrite(14, ship0302_duty_from_percent(70));

}

void ship0302_mode_right_back(int gval) {
	softPwmWrite(20, ship0302_duty_from_percent(0));
	softPwmWrite(11, ship0302_duty_from_percent(70));
}



void ship0302_mode_lift_stop(int gval) {
	softPwmWrite(14, ship0302_duty_from_percent(0));
	softPwmWrite(12, ship0302_duty_from_percent(0));

}


void ship0302_mode_right_stop(int gval) {

	softPwmWrite(20, ship0302_duty_from_percent(0));
	softPwmWrite(11, ship0302_duty_from_percent(0));

}

void ship0302_mode_stop(void) {
	digitalWrite(18, LOW); 
	softPwmWrite(14, ship0302_duty_from_percent(0));
	softPwmWrite(12, ship0302_duty_from_percent(0));
	softPwmWrite(20, ship0302_duty_from_percent(0));
	softPwmWrite(11, ship0302_duty_from_percent(0));
}

/*
 * MQTT：mode 仅 1～4（前/后/左/右），val 为业务数值；某轴 val==0 表示该轴回中。
 * 底层 ship0302_mode_* 为固定 PWM，不按 val 调速，表中只传「普通」front_val/steer_val/0。
 *
 * 前后轴 front_t：0 无  1 前  2 后
 * 左右轴 steer_t：0 无  1 左  2 右（MQTT mode 3/4 写入，与下面表 idx 不是同一套数）
 * 九格表索引 idx 不能用 front_t*3+steer_t：否则 (1,0) 纯前会得到 3，与「左」idx 冲突。
 * 应用 ship0302_dir_idx(front_t, steer_t) 映射到 0～8。
 */

typedef void (*ship0302_dir_apply_fn)(int front_val, int steer_val);

typedef struct {
	ship0302_dir_apply_fn apply;
} ship0302_dir_entry_t;

static void ship0302_dir_row_stop(int front_val, int steer_val) {
	(void)front_val;
	(void)steer_val;
	ship0302_mode_stop();
}

static void ship0302_dir_row_fwd_straight(int front_val, int steer_val) {
	(void)steer_val;
	ship0302_mode_lift_flont(front_val);
	ship0302_mode_right_flont(front_val);
}

static void ship0302_dir_row_back_straight(int front_val, int steer_val) {
	(void)steer_val;
	ship0302_mode_lift_back(front_val);
	ship0302_mode_right_back(front_val);
}

static void ship0302_dir_row_left_only(int front_val, int steer_val) {
	(void)front_val;
	ship0302_mode_right_flont(steer_val);
	ship0302_mode_lift_stop(steer_val);
}

static void ship0302_dir_row_right_only(int front_val, int steer_val) {
	(void)front_val;
	ship0302_mode_lift_flont(steer_val);
	ship0302_mode_right_stop(steer_val);
}

static void ship0302_dir_row_fwd_left(int front_val, int steer_val) {
	ship0302_mode_right_flont(steer_val);
	ship0302_mode_lift_stop(steer_val);
}

static void ship0302_dir_row_fwd_right(int front_val, int steer_val) {
	ship0302_mode_lift_flont(steer_val);
	ship0302_mode_right_stop(steer_val);
}

static void ship0302_dir_row_back_left(int front_val, int steer_val) {
	//ship0302_mode_lift_back(front_val);
	ship0302_mode_right_back(steer_val);
	ship0302_mode_lift_stop(steer_val);
}

static void ship0302_dir_row_back_right(int front_val, int steer_val) {
	ship0302_mode_lift_back(steer_val);
	ship0302_mode_right_stop(steer_val);
	//ship0302_mode_right_back(front_val);
}

/*
 * idx = ship0302_dir_idx(front_t, steer_t)
 *   idx | 方位       | front_t | steer_t
 *  -----+------------+---------+--------
 *    0  | 停         |    0    |   0
 *    1  | 前         |    1    |   0
 *    2  | 后         |    2    |   0
 *    3  | 左         |    0    |   1
 *    4  | 右         |    0    |   2
 *    5  | 前左       |    1    |   1
 *    6  | 前右       |    1    |   2
 *    7  | 后左       |    2    |   1
 *    8  | 后右       |    2    |   2
 */
static int ship0302_dir_idx(int front_t, int steer_t) {
	if (steer_t == 0) {
		return front_t;
	}
	if (front_t == 0) {
		return 2 + steer_t;
	}
	return 4 + (front_t - 1) * 2 + steer_t;
}
static const ship0302_dir_entry_t s_ship0302_dir_table[] = {
	{ ship0302_dir_row_stop },
	{ ship0302_dir_row_fwd_straight },
	{ ship0302_dir_row_back_straight },
	{ ship0302_dir_row_left_only },
	{ ship0302_dir_row_right_only },
	{ ship0302_dir_row_fwd_left },
	{ ship0302_dir_row_fwd_right },
	{ ship0302_dir_row_back_left },
	{ ship0302_dir_row_back_right },
};

#define SHIP0302_DIR_TABLE_LEN (sizeof(s_ship0302_dir_table) / sizeof(s_ship0302_dir_table[0]))

static void ship0302_apply_direction_table(int idx, int front_val, int steer_val) {
	if (idx < 0 || idx >= (int)SHIP0302_DIR_TABLE_LEN) {
		ship0302_mode_stop();
		return;
	}
	s_ship0302_dir_table[idx].apply(front_val, steer_val);
}

void ship0302_change(int *buf) {
	(void)buf[0];
	int mode = buf[1];
	int val = buf[2];

	static int ship0302_front_t = 0;
	static int ship0302_steering_t = 0;
	static int ship0302_front_val = 0;
	static int ship0302_steering_val = 0;

	pthread_mutex_lock(&s_ship0302_hw_mutex);
	s_ship0302_control_source = SHIP0302_CTRL_PHONE;

	/* 收到某轴 val==0：清该轴状态（回中） */
	if ((mode == 1 || mode == 2) && val == 0) {
		ship0302_front_t = 0;
	}
	if ((mode == 3 || mode == 4) && val == 0) {
		ship0302_steering_t = 0;
	}

	/* 前后轴 */
	if (mode == 1 && val != 0) {
		digitalWrite(18, HIGH); 
		ship0302_front_val = val;
		ship0302_front_t = 1;
	} else if (mode == 2 && val != 0) {
		digitalWrite(18, HIGH); 
		ship0302_front_t = 2;
		ship0302_front_val = val;
	}

	/* 左右轴 */
	if (mode == 3 && val != 0) {
		digitalWrite(18, HIGH); 
		ship0302_steering_t = 1;
		ship0302_steering_val = val;
	} else if (mode == 4 && val != 0) {
		digitalWrite(18, HIGH); 
		ship0302_steering_t = 2;
		ship0302_steering_val = val;
	}

	int idx = ship0302_dir_idx(ship0302_front_t, ship0302_steering_t);
	ship0302_apply_direction_table(idx, ship0302_front_val, ship0302_steering_val);
	pthread_mutex_unlock(&s_ship0302_hw_mutex);
}

/* ---------------- 0302方向盘控制（独立于手机控制） ---------------- */
#define SHIP0302_STEERING_TIMEOUT_TICKS 200

static pthread_mutex_t s_ship0302_steering_mutex = PTHREAD_MUTEX_INITIALIZER;
static int s_ship0302_steer_front_t = 0;
static int s_ship0302_steer_steering_t = 0;
static int s_ship0302_steer_front_val = 0;
static int s_ship0302_steer_steering_val = 0;
static int s_ship0302_steer_timeout = 0;
static bool s_ship0302_steer_active = false;

void ship0302_control_steering_change_process(int *buf) {
	int mode;
	int val;

	if (buf == NULL) {
		return;
	}

	mode = buf[1];
	val = buf[2];

	pthread_mutex_lock(&s_ship0302_hw_mutex);
	pthread_mutex_lock(&s_ship0302_steering_mutex);
	s_ship0302_control_source = SHIP0302_CTRL_STEERING;
	s_ship0302_steer_active = true;
	s_ship0302_steer_timeout = SHIP0302_STEERING_TIMEOUT_TICKS;

	/* 收到某轴 val==0：清该轴状态（回中） */
	if ((mode == 1 || mode == 2) && val == 0) {
		s_ship0302_steer_front_t = 0;
	}
	if ((mode == 3 || mode == 4) && val == 0) {
		s_ship0302_steer_steering_t = 0;
	}

	/* 前后轴 */
	if (mode == 1 && val != 0) {
		digitalWrite(18, HIGH);
		s_ship0302_steer_front_t = 1;
		s_ship0302_steer_front_val = val;
	} else if (mode == 2 && val != 0) {
		digitalWrite(18, HIGH);
		s_ship0302_steer_front_t = 2;
		s_ship0302_steer_front_val = val;
	}

	/* 左右轴 */
	if (mode == 3 && val != 0) {
		digitalWrite(18, HIGH);
		s_ship0302_steer_steering_t = 1;
		s_ship0302_steer_steering_val = val;
	} else if (mode == 4 && val != 0) {
		digitalWrite(18, HIGH);
		s_ship0302_steer_steering_t = 2;
		s_ship0302_steer_steering_val = val;
	}

	{
		int idx = ship0302_dir_idx(s_ship0302_steer_front_t, s_ship0302_steer_steering_t);
		ship0302_apply_direction_table(idx, s_ship0302_steer_front_val, s_ship0302_steer_steering_val);
	}

	pthread_mutex_unlock(&s_ship0302_steering_mutex);
	pthread_mutex_unlock(&s_ship0302_hw_mutex);
}

void ship0302_steering_smooth_all(void) {
	pthread_mutex_lock(&s_ship0302_hw_mutex);
	pthread_mutex_lock(&s_ship0302_steering_mutex);

	/* 仅方向盘控制源下执行；手机控制时线程不写硬件 */
	if (s_ship0302_control_source != SHIP0302_CTRL_STEERING || !s_ship0302_steer_active) {
		pthread_mutex_unlock(&s_ship0302_steering_mutex);
		pthread_mutex_unlock(&s_ship0302_hw_mutex);
		return;
	}

	s_ship0302_steer_timeout--;
	if (s_ship0302_steer_timeout <= 0) {
		s_ship0302_steer_front_t = 0;
		s_ship0302_steer_steering_t = 0;
		s_ship0302_steer_front_val = 0;
		s_ship0302_steer_steering_val = 0;
		s_ship0302_steer_active = false;
		s_ship0302_control_source = SHIP0302_CTRL_NONE;
		ship0302_mode_stop();
		pthread_mutex_unlock(&s_ship0302_steering_mutex);
		pthread_mutex_unlock(&s_ship0302_hw_mutex);
		return;
	}

	{
		int idx = ship0302_dir_idx(s_ship0302_steer_front_t, s_ship0302_steer_steering_t);
		ship0302_apply_direction_table(idx, s_ship0302_steer_front_val, s_ship0302_steer_steering_val);
	}

	pthread_mutex_unlock(&s_ship0302_steering_mutex);
	pthread_mutex_unlock(&s_ship0302_hw_mutex);
}
