/*
 * servo-control.hpp
 *
 *  Created on: 12.01.2021
 *      Author: frank
 */

#include <ax12.h>
#include "noise.hpp"
#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise



#ifndef SERVO_CONTROL_HPP_
#define SERVO_CONTROL_HPP_

#define SERVO_COUNT 5

struct Mood{
  float shakiness;
  float wakefulness;
  float loneliness;
};
extern Mood mood;

#define RPM_TO_SPEED_RATIO 0.111
#define ANGLE_PER_DIGIT_RATIO .2925
//#define ANGLE_PER_DIGIT_RATIO 0.29			// From datasheet

#define MOTOR_TORQUE_LIMIT 100
#define MOTOR_TORQUE_RATIO 0.1
#define MOTOR_TORQUE_DIRECTION_MASK 0x00000200
#define MOTOR_TORQUE_VALUE_MASK 0x000001ff

#define BASE_MOTOR_INITIAL_POSITION_DEG 150
#define BASE_MOTOR_INITIAL_SPEED_RPM 5
#define BASE_MOTOR_MAX_ANGLE_DEG 48
#define BASE_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT

#define MID_MOTOR_INITIAL_POSITION_DEG 150
#define MID_MOTOR_INITIAL_SPEED_RPM 7
#define MID_MOTOR_MAX_ANGLE_DEG 105
#define MID_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT

#define NOSE_MOTOR_INITIAL_POSITION_DEG 150
#define NOSE_MOTOR_INITIAL_SPEED_RPM 10
#define NOSE_MOTOR_MAX_ANGLE_DEG 105
#define NOSE_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT

#define DXL_BASE_MOTOR_INITIAL_POSITION BASE_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO
#define DXL_BASE_MOTOR_INITIAL_SPEED BASE_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO
#define DXL_BASE_MOTOR_MAX_ANGLE BASE_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO
#define DXL_BASE_MOTOR_MIN_POSITION_CW DXL_BASE_MOTOR_INITIAL_POSITION - DXL_BASE_MOTOR_MAX_ANGLE
#define DXL_BASE_MOTOR_MAX_POSITION_CCW DXL_BASE_MOTOR_INITIAL_POSITION + DXL_BASE_MOTOR_MAX_ANGLE
#define DXL_BASE_MOTOR_TORQUE_LIMIT BASE_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO

#define DXL_MID_MOTOR_INITIAL_POSITION MID_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO
#define DXL_MID_MOTOR_INITIAL_SPEED MID_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO
#define DXL_MID_MOTOR_MAX_ANGLE MID_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO
#define DXL_MID_MOTOR_MIN_POSITION_CW DXL_MID_MOTOR_INITIAL_POSITION - DXL_MID_MOTOR_MAX_ANGLE
#define DXL_MID_MOTOR_MAX_POSITION_CCW DXL_MID_MOTOR_INITIAL_POSITION + DXL_MID_MOTOR_MAX_ANGLE
#define DXL_MID_MOTOR_TORQUE_LIMIT MID_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO

#define DXL_NOSE_MOTOR_INITIAL_POSITION NOSE_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO
#define DXL_NOSE_MOTOR_INITIAL_SPEED NOSE_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO
#define DXL_NOSE_MOTOR_MAX_ANGLE NOSE_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO
#define DXL_NOSE_MOTOR_MIN_POSITION_CW DXL_NOSE_MOTOR_INITIAL_POSITION - DXL_NOSE_MOTOR_MAX_ANGLE
#define DXL_NOSE_MOTOR_MAX_POSITION_CCW DXL_NOSE_MOTOR_INITIAL_POSITION + DXL_NOSE_MOTOR_MAX_ANGLE
#define DXL_NOSE_MOTOR_TORQUE_LIMIT NOSE_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO

struct dxl_servo {
	int id;
	int initial_speed;
	int initial_position;
	int min_position;
	int max_position;
	int torque_limit;
	int current_torque;
	int joint_orientation;
};

void matrix_weights_update();

void servos_setup();
void servos_set_current_initial_position();

void servos_init_with_fixed_position();
void servos_init_with_current_position();

void servo_init(const int servo_id);
void servos_init();

void servo_to_initial_position(const int servo_id);
void servos_to_initial_position();

int servo_moving(const int servo_id);
int servos_moving();

void servos_in_position_wait();
void servo_in_position_wait(int servo_id);

int servo_set_speed_rpm(const int servo_id, const int rpm);

void servo_move_to_angle(const int servo_id, const int deg);
void servos_move_to_angle(const int deg);

void servos_collect_current_torque();
void servos_print_current_torque();

void map_roll_to_servos_and_move(const int roll);
void map_pitch_to_servos_and_move(const int pitch);

void map_angle_to_servo_with_initial_position(const int servo_id, const int deg);
void map_roll_to_servos_with_initial_position_and_move(const int roll);
void map_pitch_to_servos_with_initial_position_and_move(const int pitch);

void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch);

void read_servos_positions();

#endif /* SERVO_CONTROL_HPP_ */