#include "Arduino.h"
#include "behaviour.h"

SimplexNoise sn;


Mood mood;

float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods
float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 

void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll){
  mood.loneliness=1.0;
  
  static unsigned long last_weights_update=0;
  if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
    last_weights_update=millis;

    static float contact_main_persist=0;
    if (contact_main) {
      contact_main_persist=1.0;
    }else{
      if (contact_main_persist>0) {
        contact_main_persist-=0.2*WEIGHT_UPDATE_INTERVAL/1000.0;
      }else{
        contact_main_persist=0;
      }
    }
    
    //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;*/


    bool state_bellyup=false;
    bool state_map_mode_detached=false;
    bool state_map_mode_attached=false;

    float map_mode_rollpitch=0;
    float map_mode_slither=0;
    float map_mode_sleeping=0;


    float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0);
    float map_mode_noise=1.0-contact_main_smooth; //idle movement
    float map_mode_scared=0.0+contact_main_smooth; //contracted position


    
    


    float map_sum=map_mode_noise+map_mode_scared;
    map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
    map_mode_scared/=map_sum;

    
    //Mode Noise
    matrix_weights_main[0][W_NOISE]=30.0 *map_mode_noise;
    matrix_weights_main[0][W_NOISESLOW]=0.0 *map_mode_noise;
    
    matrix_weights_main[1][W_NOISE]=30.0 *map_mode_noise;
    matrix_weights_main[1][W_NOISESLOW]=20.0 *map_mode_noise;
    
    matrix_weights_main[2][W_NOISE]=20.0 *map_mode_noise;
    matrix_weights_main[2][W_NOISESLOW]=30.0 *map_mode_noise;
    
    matrix_weights_main[0][W_LOOKDIRECTION]=30* map_mode_scared; 
    matrix_weights_main[1][W_LOOKDIRECTION]=30.0* map_mode_scared;  
    matrix_weights_main[2][W_LOOKDIRECTION]=30.0* map_mode_scared; 

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

    //Mode Slither
    matrix_weights_main[0][W_COS]=30 *map_mode_slither;
    matrix_weights_main[1][W_SIN]=30 *map_mode_slither;
    matrix_weights_main[2][W_COS]=-30 *map_mode_slither;
    
    //Mode Sleeping
    matrix_weights_main[0][W_NOISESLOW]=20 *map_mode_sleeping;
    matrix_weights_main[1][W_NOISESLOW]=20 *map_mode_sleeping;
    matrix_weights_main[2][W_NOISESLOW]=30 *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);
    }
    */
  }
}


void updateServosByWeights(unsigned long millis){
  static unsigned long last_servoweights_update=0;
  if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL) 
  {
    last_servoweights_update=millis;
    static unsigned long millis_add;
    #define MILLISADD_MAX 100
    millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);

    float pitch=0;
    float roll=0;

    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++) {
      uint8_t servoPosInMatrix=0;

      
      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
      

      float angle=0;

      float lookdirection=(servos[i].angle>=0)?1:-1; //if angle positive, 1, else 0
      
      
      

      if (i<SERVO_COUNT_MAIN) { //Main
        /*if (i==0){
          Serial.print("servoangle:");Serial.println(servos[i].angle);
          Serial.print("lookdirection:");Serial.println(lookdirection);
          Serial.print("weight:");Serial.println(matrix_weights_main[i][W_LOOKDIRECTION]);


                    
        }*/

        servoPosInMatrix=i;
        angle=
          pitch*matrix_weights_main[servoPosInMatrix][W_PITCH]
          +roll*matrix_weights_main[servoPosInMatrix][W_ROLL]
          +pnoise*matrix_weights_main[servoPosInMatrix][W_NOISE]
          +pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW]
          +vsin*matrix_weights_main[servoPosInMatrix][W_SIN]
          +vcos*matrix_weights_main[servoPosInMatrix][W_COS]
          +lookdirection*matrix_weights_main[servoPosInMatrix][W_LOOKDIRECTION];
        
      }else if(i<SERVO_COUNT_MAIN+SERVO_COUNT_PERIPHERAL) { //Peripheral Left
        servoPosInMatrix=i-3;
      }else if(i<SERVO_COUNT_MAIN+2*SERVO_COUNT_PERIPHERAL) { //Peripheral Right
        servoPosInMatrix=i-2*3;

      }
      
      servos[i].angle=angle;
      
    }
  }
  
}



float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}