#include <main.h>
#include <stdint.h>
#include <dynamixel1.h>
#include <stdio.h>

#define PARAMETERS_OFFSET 5

extern UART_HandleTypeDef huart2;
extern ADC_HandleTypeDef hadc1;

uint8_t counter = 0;

typedef enum { FSM_H1, FSM_H2, FSM_ID, FSM_LEN, FSM_INST, FSM_PARAMETER, FSM_CKSM } FsmState_t;
typedef struct {
	FsmState_t state;
	int count;
	int cksm;
}Fsm_t;

static Fsm_t fsm = { .state = FSM_H1, .count = 0, .cksm = 0 }; 

void dynamixel1_tick()
{
	counter++;
	if (counter > 2 && fsm.state != FSM_H1)
	{
		counter = 0;
		fsm.state = FSM_H1;
	}
}

void dynamixel1_send_status_packet(Error_t error, uint8_t parameters[], uint8_t len) {
	uint8_t status_packet[10] = { 0xFF, 0xFF, IDSENS, len + 2, error };
	uint8_t cksm = IDSENS + error + 2 + len;
	for (int i = 0; i < len; i++) 
	{
		cksm = cksm + parameters[i];
		status_packet[PARAMETERS_OFFSET + i] = parameters[i];
	}
	cksm = cksm & 0xFF;
	cksm = 0xFF - cksm;
	
	status_packet[PARAMETERS_OFFSET + len] = cksm;

	HAL_UART_Transmit(&huart2, status_packet, PARAMETERS_OFFSET + 1 + len, 1);
	}

PacketState_t dynamixel1_byte_recieved(uint8_t value,Packet_t* packet)
{
	printf("i: %X", value & 0xff);
	counter = 0;
	switch (fsm.state)
	{
	case FSM_H1:
		if (value == 0xFF)
		{
			fsm.count = 0;
			fsm.cksm = 0;			
			fsm.state = FSM_H2;
		}
		break;
	case FSM_H2:
		if (value == 0xFF)
		{
			fsm.state = FSM_ID;
		}
		break;
	case FSM_ID:
		packet->ID = value;
		if (IDSENS != value)
		{
			fsm.state = FSM_H1;
			break;
		}
		fsm.state = FSM_LEN;
		fsm.cksm = value;
		
		break;
	case FSM_LEN:
		packet->LEN = value;
		fsm.state = FSM_INST;
		fsm.cksm = fsm.cksm + value;
		break;
	case FSM_INST:
		packet->INST = value;
		fsm.state = FSM_PARAMETER;
		fsm.cksm = fsm.cksm + value;
		break;
	case FSM_PARAMETER:
		packet->parameters[fsm.count] = value;
		fsm.count++;
		fsm.cksm = fsm.cksm + value;
	
		if (fsm.count == packet->LEN-2)
		{
			fsm.state = FSM_CKSM;
		}
		break;
	case FSM_CKSM:
		fsm.cksm = fsm.cksm & 0xFF;
		fsm.cksm = 0xFF - fsm.cksm;
		if (value == fsm.cksm)
		{
			fsm.state = FSM_H1;
			return PACKET_OK;
		}
		else
		{
			return CKSM_ERROR;
		}
		break;
	}
	return MORE_BYTES;
}

