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

#include "servo-control.hpp"

SimplexNoise sn;


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

  float map_mode_noise=constrain(mapfloat(mood.wakefulness,0,0.2, 0,1),0,1);
  float map_mode_rollpitch=constrain(mapfloat(mood.shakiness,0,0.5, 0,1),0,1);
  float map_mode_slither=0;
  float map_mode_sleeping=mood.loneliness;

  
  


  float map_sum=map_mode_noise+map_mode_rollpitch+map_mode_slither+map_mode_sleeping;
  map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
  map_mode_rollpitch/=map_sum;
  map_mode_slither/=map_sum;
  map_mode_sleeping/=map_sum;

  /*
  Serial.print(map_mode_noise); Serial.print(",");
  Serial.print(map_mode_rollpitch); Serial.print(",");
  Serial.print(map_mode_slither); Serial.print(",");
  Serial.print(map_mode_sleeping); Serial.println();
  */
  
  //Mode Noise
  matrix_weights[0][W_NOISE]=90.0 *map_mode_noise;
  matrix_weights[0][W_NOISESLOW]=0.0 *map_mode_noise;
  
  matrix_weights[1][W_NOISE]=70.0 *map_mode_noise;
  matrix_weights[1][W_NOISESLOW]=20.0 *map_mode_noise;
  
  matrix_weights[2][W_NOISE]=50.0 *map_mode_noise;
  matrix_weights[2][W_NOISESLOW]=40.0 *map_mode_noise;
  
  matrix_weights[3][W_NOISE]=20.0 *map_mode_noise;
  matrix_weights[3][W_NOISESLOW]=70.0 *map_mode_noise;
  
  matrix_weights[4][W_NOISE]=0.0 *map_mode_noise;
  matrix_weights[4][W_NOISESLOW]=60.0 *map_mode_noise;

  //Mode Roll Pitch
  matrix_weights[0][W_ROLL]=1.0* map_mode_rollpitch; 
  matrix_weights[1][W_PITCH]=1.0* map_mode_rollpitch;  
  matrix_weights[2][W_ROLL]=1.0* map_mode_rollpitch;  
  matrix_weights[3][W_PITCH]=1.0* map_mode_rollpitch;  
  matrix_weights[4][W_ROLL]=1.0* map_mode_rollpitch;

  //Mode Slither
  matrix_weights[0][W_COS]=30 *map_mode_slither;
  matrix_weights[1][W_SIN]=30 *map_mode_slither;
  matrix_weights[2][W_COS]=-30 *map_mode_slither;
  matrix_weights[3][W_SIN]=-30 *map_mode_slither;
  matrix_weights[4][W_COS]=30 *map_mode_slither;
  
  //Mode Sleeping
  matrix_weights[0][W_NOISESLOW]=20 *map_mode_sleeping;
  matrix_weights[1][W_NOISESLOW]=20 *map_mode_sleeping;
  matrix_weights[2][W_NOISESLOW]=30 *map_mode_sleeping;
  matrix_weights[3][W_NOISESLOW]=15 *map_mode_sleeping;
  matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping;

  /*
  float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 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;
    matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
  }
  */
  

	
  
}

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);
  
  //unsigned long blastart=micros();
  for (uint8_t i=0;i<SERVO_COUNT;i++) {
    //unsigned long blastart=micros();
    //float pnoise=PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
    //float pnoisesslow=0;//PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
    //Serial.print("perlin micros="); Serial.println(micros()-blastart);

    
    float pnoise=sn.noise((millis()+millis_add)/5000.0,i*10); //simplexnoise -1 to 1
    float pnoiseslow=sn.noise((millis()+millis_add)/50000.0,i*10); //simplexnoise -1 to 1
    

    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);
  //Serial.print("setgoalposition micros="); Serial.println(micros()-blastart);
  
}

#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
void read_servos_positions()
{
  int errors[SERVO_COUNT];
  Serial.print("dxlgp");
  for (uint8_t i=0;i<SERVO_COUNT;i++) {
    //Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
    
    Serial.print(",");
    
    Serial.print(dxlGetPosition(servos[i].id)); 
    errors[i]=ax12GetLastError();
    
    
    
  }
  Serial.println();

  Serial.print("error");
  for (uint8_t i=0;i<SERVO_COUNT;i++) {
    Serial.print(",");    
    Serial.print(errors[i]); 
  }
  Serial.println();

  

  for(int i = 0; i< SERVO_COUNT; i++) {
    int errorcode=errors[i];
    if (errorcode!=0) {
      Serial.print("Error present on "); Serial.print(i); Serial.print("errorcode="); Serial.println(errorcode);
    }
    if (CHECK_BIT(errorcode,5)) { //overload error
      Serial.print("Init Servo "); Serial.println(i);
      delay(1000);
      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);

      
      
    }
  }
}


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("]");
}