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

#include "servo-control.hpp"


int angle_to_dxl_servo_position(const int deg);
int rpm_to_dxl_servo_speed(const int rpm);


Mood mood;

float matrix_weights[SERVO_COUNT][6]; //mapping outputs, sensors/moods
#define W_PITCH 0
#define W_ROLL 1
#define W_NOISE 2
#define W_NOISESLOW 3
#define W_SIN 4
#define W_COS 5


void matrix_weights_update(){
  //0=pitch
  //1=roll
  

  float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
  //float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise

  
  
  matrix_weights[0][W_COS]=40.0;
  

  //testing on servo
  /*
  matrix_weights[0][W_COS]=30 *map_mode;
  matrix_weights[0][W_NOISE]=90.0 *map_mode;
  matrix_weights[1][W_SIN]=30 *map_mode;
  matrix_weights[1][W_NOISE]=90.0 *map_mode;
  matrix_weights[2][W_COS]=-30 *map_mode;
  matrix_weights[2][W_NOISE]=90.0 *map_mode;
  matrix_weights[3][W_SIN]=-30 *map_mode;
  matrix_weights[3][W_NOISE]=90.0 *map_mode;
  matrix_weights[4][W_COS]=30 *map_mode;
  matrix_weights[4][W_NOISE]=90.0 *map_mode;


  matrix_weights[0][W_ROLL]=1.0* (1-map_mode); 
  matrix_weights[1][W_PITCH]=1.0* (1-map_mode);  
  matrix_weights[2][W_ROLL]=1.0* (1-map_mode);  
  matrix_weights[3][W_PITCH]=1.0* (1-map_mode);  
  matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
  */

  /*
  matrix_weights[0][W_NOISE]=90.0*map_mode;
  matrix_weights[0][W_NOISESLOW]=90.0*map_mode;
  matrix_weights[1][W_NOISE]=90.0*map_mode;
  matrix_weights[1][W_NOISESLOW]=90.0*map_mode;
  matrix_weights[2][W_NOISE]=90.0*map_mode;
  matrix_weights[2][W_NOISESLOW]=90.0*map_mode;
  matrix_weights[3][W_NOISE]=90.0*map_mode;
  matrix_weights[3][W_NOISESLOW]=90.0*map_mode;
  matrix_weights[4][W_NOISE]=90.0*map_mode;
  matrix_weights[4][W_NOISESLOW]=90.0*map_mode;

  
  */

  
  float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 0,1),0,1);
  for(int i = 0; i < SERVO_COUNT ; i++){
    matrix_weights[i][W_PITCH]*=sleeping;
    matrix_weights[i][W_ROLL]*=sleeping;
    matrix_weights[i][W_NOISE]*=sleeping;
  }
  

	
  
}

dxl_servo servos[SERVO_COUNT];

void servos_set_current_initial_position() {
	for(int i = 0; i < SERVO_COUNT ; i++){
		servos[i].initial_position = dxlGetPosition(servos[i].id);
	}
}

void servos_setup() {
	servos[0].id = 1;
	servos[0].initial_position = DXL_NOSE_MOTOR_INITIAL_POSITION;
	servos[0].initial_speed = DXL_NOSE_MOTOR_INITIAL_SPEED;
	servos[0].max_position = DXL_NOSE_MOTOR_MAX_POSITION_CCW;
	servos[0].min_position = DXL_NOSE_MOTOR_MIN_POSITION_CW;
	servos[0].torque_limit = DXL_NOSE_MOTOR_TORQUE_LIMIT;
	servos[0].joint_orientation = - 1;

	servos[1].id = 2;
	servos[1].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
	servos[1].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
	servos[1].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
	servos[1].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
	servos[1].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
	servos[1].joint_orientation = - 1;

	servos[2].id = 3;
	servos[2].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
	servos[2].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
	servos[2].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
	servos[2].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
	servos[2].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
	servos[2].joint_orientation = + 1;

	servos[3].id = 4;
	servos[3].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
	servos[3].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
	servos[3].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
	servos[3].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
	servos[3].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
	servos[3].joint_orientation = + 1;

	servos[4].id = 5;
	servos[4].initial_position = DXL_BASE_MOTOR_INITIAL_POSITION;
	servos[4].initial_speed = DXL_BASE_MOTOR_INITIAL_SPEED;
	servos[4].max_position = DXL_BASE_MOTOR_MAX_POSITION_CCW;
	servos[4].min_position = DXL_BASE_MOTOR_MIN_POSITION_CW;
	servos[4].torque_limit = DXL_BASE_MOTOR_TORQUE_LIMIT;
	servos[4].joint_orientation = - 1;
}

void servos_init_with_fixed_position() {
	servos_setup();
	servos_init();
}

void servos_init_with_current_position() {
	servos_setup();
	servos_set_current_initial_position();
	servos_init();
}

void servos_init() {
	for(int i = 0; i< SERVO_COUNT; i++) {
    dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
		dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
		dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
		dxlTorqueOn(servos[i].id);
		dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
		dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
	}
}

void servo_init(const int servo_id) {
	int index = servo_id - 1;
	dxlSetJointMode(servos[index].id, servos[index].min_position, servos[index].max_position);
	dxlSetGoalSpeed(servos[index].id, servos[index].initial_speed);
	dxlSetGoalPosition(servos[index].id, servos[index].initial_position);
}

void servo_to_initial_position(const int servo_id) {
	dxlSetGoalPosition(servo_id, DXL_MID_MOTOR_INITIAL_POSITION);
}

void servos_to_initial_position() {
	for(int i = 0; i < SERVO_COUNT; i++) {
		dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
	}
}

int servo_moving(const int servo_id) {
	return dxlGetMoving(servo_id);
}

int servos_moving() {
	int counter = 0;
	for(int i = 0; i < SERVO_COUNT; i++) {
		counter += dxlGetMoving(servos[i].id);
	}

	if(counter == SERVO_COUNT) {
		return 1;
	} else {
		return 0;
	}
}

void servos_in_position_wait() {
	while(servos_moving() == 1);
}

void servo_in_position_wait(const int servo_id) {
	while(dxlGetMoving(servo_id));
}


int servo_set_speed_rpm(const int servo_id, const int rpm) {
	dxlSetGoalSpeed(servo_id, rpm);
	return rpm;
}

void servo_move_to_angle(const int servo_id, const int deg) {
	dxlSetGoalPosition(servo_id, angle_to_dxl_servo_position(deg));
}

void map_angle_to_servo_with_initial_position(const int servo_id, const int deg) {
	dxlSetGoalPosition(servo_id, servos[servo_id -1].initial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg));
}

void servos_move_to_angle(const int deg) {
	for(int i = 0; i < SERVO_COUNT; i++) {
		dxlSetGoalPosition(servos[i].id, angle_to_dxl_servo_position(deg));
	}
}

int angle_to_dxl_servo_position(const int deg) {
	return deg / ANGLE_PER_DIGIT_RATIO;
}

int rpm_to_dxl_servo_speed(const int rpm) {
	return rpm / RPM_TO_SPEED_RATIO;
}

void map_roll_to_servos_and_move(const int roll){
	dxlSetGoalPosition(servos[0].id, servos[0].joint_orientation * angle_to_dxl_servo_position(roll));
	dxlSetGoalPosition(servos[2].id, servos[2].joint_orientation * angle_to_dxl_servo_position(roll));
	dxlSetGoalPosition(servos[4].id, servos[4].joint_orientation * angle_to_dxl_servo_position(roll));
  delay(1);
}

void map_pitch_to_servos_and_move(const int pitch){
	dxlSetGoalPosition(servos[1].id, servos[1].joint_orientation * angle_to_dxl_servo_position(pitch));
	dxlSetGoalPosition(servos[3].id, servos[3].joint_orientation * angle_to_dxl_servo_position(pitch));
 delay(1);
}

void map_roll_to_servos_with_initial_position_and_move(const int roll){
	dxlSetGoalPosition(servos[0].id, servos[0].initial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll)));
	dxlSetGoalPosition(servos[2].id, servos[2].initial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll)));
	dxlSetGoalPosition(servos[4].id, servos[4].initial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll)));
  delay(1);
}

void map_pitch_to_servos_with_initial_position_and_move(const int pitch){
	dxlSetGoalPosition(servos[1].id, servos[1].initial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch)));
	dxlSetGoalPosition(servos[3].id, servos[3].initial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch)));
  delay(1);
}

void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch){
  
  static unsigned long millis_add;
  #define MILLISADD_MAX 100
  millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
  //Serial.print(mood.shakiness);
  //Serial.print(", ");
  //Serial.println(millis_add);
  float vsin=sin((millis()+millis_add)/1000.0);
  float vcos=cos((millis()+millis_add)/1000.0);
  
  for (uint8_t i=0;i<=SERVO_COUNT;i++) {
    //unsigned long blastart=micros();
    float pnoise=0;//PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
    float pnoiseslow=0;//PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
    //Serial.print("perlin micros="); Serial.println(micros()-blastart);

    int angle=0;
    angle=
      pitch*matrix_weights[i][W_PITCH]
      +roll*matrix_weights[i][W_ROLL]
      +pnoise*matrix_weights[i][W_NOISE]
      +pnoiseslow*matrix_weights[i][W_NOISESLOW]
      +vsin*matrix_weights[i][W_SIN]
      +vcos*matrix_weights[i][W_COS];
    
    
    dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));   
    
    
  }
  //delay(1);
  
}


int servo_calculate_torque_with_direction(const int servo_id) {
	int current_torque = dxlGetTorque(servo_id);
	int direction = current_torque & MOTOR_TORQUE_DIRECTION_MASK;
	current_torque &= MOTOR_TORQUE_VALUE_MASK;
	current_torque *= MOTOR_TORQUE_RATIO;
	if(direction == 0) {
		current_torque *= -1;
	}
	return current_torque;
}

void servos_collect_current_torque() {
	for(int i = 0; i < SERVO_COUNT; i++) {
		servos[i].current_torque = servo_calculate_torque_with_direction(servos[i].id);
	}
}

void servos_print_current_torque() {
	Serial.print("s_t=[");
	Serial.print(servos[SERVO_COUNT - 1].current_torque);
	for(int i=SERVO_COUNT - 2; i >= 0; i--) {
		Serial.print(",");
		Serial.print(servos[i].current_torque);
	}
	Serial.println("]");
}