`timescale 1ns/1ps

module controller #(parameter N=21)
   (input logic clk,
    input logic reset,

    input logic sensor_l,
    input logic sensor_m,
    input logic sensor_r,

    input logic [N-1:0] count_in,
    output logic count_reset,

    output logic motor_l_reset,
    output logic motor_l_direction,

    output logic motor_r_reset,
    output logic motor_r_direction);

typedef enum logic [2:0] {Central, Forward, Gentle_l, Sharp_l, Gentle_r, Sharp_r} control_state;
control_state state, next_state;

always_ff @(posedge clk, posedge count_reset, posedge reset) begin
	if(count_reset | reset)
		state <= Central;
	else 
		state <= next_state;
end

logic [2:0] sensors_lmr;

assign sensors_lmr = {sensor_l, sensor_m, sensor_r};

always_comb begin
	if (count_in >= 21'd2_000_000 | reset)
		count_reset = 1;
	else
		count_reset = 0;

	case(state)
		Central: begin
			motor_l_reset = 1;
			motor_l_direction = 0;
			motor_r_reset = 1;
			motor_r_direction = 0;

			case(sensors_lmr)
				3'b000, 3'b010, 3'b101, 3'b111: next_state = Forward;
				3'b001: next_state = Gentle_l;
				3'b011: next_state = Sharp_l;
				3'b100: next_state = Gentle_r;
				3'b110: next_state = Sharp_r;
				default: next_state = Central;
			endcase
		end
		Forward: begin
			motor_l_reset = 0;
			motor_l_direction = 1;
			motor_r_reset = 0;
			motor_r_direction = 0;

			next_state = state;
		end
		Gentle_l: begin
			motor_l_reset = 1;
			motor_l_direction = 0;
			motor_r_reset = 0;
			motor_r_direction = 0;

			next_state = state;
		end
		Sharp_l: begin
			motor_l_reset = 0;
			motor_l_direction = 0;
			motor_r_reset = 0;
			motor_r_direction = 0;

			next_state = state;
		end
		Gentle_r: begin
			motor_l_reset = 0;
			motor_l_direction = 1;
			motor_r_reset = 1;
			motor_r_direction = 0;

			next_state = state;
		end
		Sharp_r: begin
			motor_l_reset = 0;
			motor_l_direction = 1;
			motor_r_reset = 0;
			motor_r_direction = 1;

			next_state = state;
		end
		default: begin
			motor_l_reset = 1;
			motor_l_direction = 0;
			motor_r_reset = 1;
			motor_r_direction = 0;

			next_state = state;
		end
	endcase

end


endmodule