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

SimplexNoise sn;


Mood mood;

float map_mode_rollpitch=0;
float map_mode_walking=0;
float map_mode_sleeping=0;
float map_mode_noise=0;

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 initBehaviour() {
  mood.arousal=0;
  mood.valence=0;
}


void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,float maxAccX,float maxAccY,float maxAccZ,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR,float height,float temp_ambient,float temp_object,float pumpspeed, float servoerror) {
  static unsigned long last_mood_update=0;
  if (millis-last_mood_update>MOOD_UPDATE_INTERVAL) {
    last_mood_update=millis;
    

    float step=MOOD_UPDATE_INTERVAL/1000.0;

    if (body_present && pumpspeed<0.8) { //body present and vacuum present (pump below speed 1.0)
      if (mood.valence<0.0) { //when attached, and valence low
        mood.valence=0.2; //jump valence up quickly
      }
      if (mood.valence<0.5) { //increase valence up to a certain point
        mood.valence+=step*0.05;
      }
    }
    if (!body_present) {
      if (mood.valence>-0.5) { //decrease valence down to a certain point
        mood.valence-=step*0.01;
      }
    }

    static unsigned long last_contact_time=0;
    static bool last_contact_main=false;
    static bool last_contact_peripheralR=false;
    static bool last_contact_peripheralL=false;
    

    if (contact_main || contact_peripheralL || contact_peripheralR) {
      if ((contact_main && !last_contact_main) ||  (contact_peripheralL && !last_contact_peripheralL) || (contact_peripheralR && !last_contact_peripheralR)) { //on first contact

        long _contacttime_min=1000;
        long _contacttime_max=20000;
        float _contact_increase_arousal=0.7;
        float _contact_increase_valence=0.1;
        if (mood.valence<-0.2) {
          long _contacttime_min=500;
          long _contacttime_max=2000;
          float _contact_increase_arousal=0.7;
          float _contact_increase_valence=-0.2;
        }
        float olda=mood.arousal;
        float oldv=mood.valence;
        mood.arousal+=constrain(mapfloat(millis-last_contact_time,_contacttime_min,_contacttime_max,0,_contact_increase_arousal),0,_contact_increase_arousal);
        mood.valence+=constrain(mapfloat(millis-last_contact_time,_contacttime_min,_contacttime_max,0,_contact_increase_valence),0,_contact_increase_valence);
        Serial.print("Contact. increase arousal:"); Serial.print(mood.arousal-olda); Serial.print(" valence:"); Serial.print(mood.valence-oldv); Serial.println();
      }
      last_contact_time=millis;
    }

    last_contact_main=contact_main;
    last_contact_peripheralR=contact_peripheralR;
    last_contact_peripheralL=contact_peripheralL;


    static float last_height=0;
    static float heightdiffsmooth=0; //goes up to 0.2 when lifted or lowered quickly 1m and slowly deacreases to 0
    heightdiffsmooth+=height-last_height;
    if (heightdiffsmooth>0) {
      heightdiffsmooth-=behaviour_confortable_lift*step;
    }else if (heightdiffsmooth<0) {
      heightdiffsmooth+=behaviour_confortable_lift*step; //0.1*step means 0.1m of heightchange per second does not affect heightdiffsmooth
    }
    
    //Serial.print("heightdiffsmooth:"); Serial.println(heightdiffsmooth,3);

    
    float maxAcc=sqrt(pow(maxAccX,2)+pow(maxAccY,2)+pow(maxAccZ,2));
    float preferredMaxAcc=0.5;
    if (constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)>0.0) {
      digitalWrite(PIN_LED,LOW); //On
    }else{
      digitalWrite(PIN_LED,HIGH); //Off
    }
    if (mood.arousal<-0.5) {
      mood.arousal+=constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)*step*0.2;
    }else{
      mood.valence+=constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)*step*0.2;
      if (mood.arousal>0.7) {
        mood.arousal-=step*0.04;
      }else if(mood.arousal<0.3) {
        mood.arousal+=step*0.03;
      }
    }

    if (abs(heightdiffsmooth)>0.05) {
      mood.valence-=abs(heightdiffsmooth)*step*1.0;
      if (mood.arousal<0.9){
        mood.arousal+=abs(heightdiffsmooth)*step*3.0;
      }
    }

    last_height=height;


    mood.arousal-=step*0.02; //slowly deacrease arousal

    if (mood.valence<0) {
      mood.valence+=step*0.03;
    }else if (mood.valence>0) {
      mood.valence-=step*0.01;
    }
    

    mood.arousal=constrain(mood.arousal,-1.0,1.0);
    mood.valence=constrain(mood.valence,-1.0,1.0);

  }
}

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

    

    
    //### Update Mode ###
    

    float aim_map_mode_rollpitch=0;
    float aim_map_mode_walking=0;
    float aim_map_mode_sleeping=0;
    float aim_map_mode_noise=0;

    

    if (body_present) { //attached    
      
      aim_map_mode_rollpitch=constrain(mapfloat(mood.valence,-0.3,0.3,0,1.0),0,1.0);
      
      aim_map_mode_noise=constrain(mapfloat(mood.valence,-0.2,-1.0,0,1.0),0,1.0);
      

    }else{ //detached
      if (bellyup) { //belly up
        aim_map_mode_walking=1.0;
      }else{ //sitting on a level survace
        aim_map_mode_noise=constrain(mapfloat(mood.arousal,-0.5,0.5, 0.0,1.0),0,1.0);
        map_mode_sleeping=constrain(mapfloat(mood.arousal,-1.0,-0.6, 1.0,0.0),0,1.0);
      }
    }
    
    map_mode_rollpitch=interpolateTo(map_mode_rollpitch,aim_map_mode_rollpitch,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
    map_mode_walking=interpolateTo(map_mode_walking,aim_map_mode_walking,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
    map_mode_sleeping=interpolateTo(map_mode_sleeping,aim_map_mode_sleeping,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
    map_mode_noise=interpolateTo(map_mode_noise,aim_map_mode_noise,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);


    
    
    

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

    analogWrite(PIN_VIBRATION,contact_main?64:0);
    

    //reset all weights
    for (uint8_t i=0;i<SERVO_COUNT_MAIN;i++) {
      for (uint8_t w=0;w<WEIGHT_COUNT;w++) {
        matrix_weights_main[i][w]=0;
      }
    }
    for (uint8_t i=0;i<SERVO_COUNT_PERIPHERAL;i++) {
      for (uint8_t w=0;w<WEIGHT_COUNT;w++) {
        matrix_weights_peripheralL[i][w]=0;
        matrix_weights_peripheralR[i][w]=0;
      }
    }
    
    //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;
    

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

    //Mode Walking
    matrix_weights_main[0][W_NOISESLOW]+=5 *map_mode_walking;
    matrix_weights_main[0][W_SIN]+=10 *map_mode_walking;
    matrix_weights_main[0][W_OFFSET]+=-60 *map_mode_walking;
    matrix_weights_main[1][W_NOISESLOW]+=30 *map_mode_walking;
    matrix_weights_main[2][W_COS]+=-20 *map_mode_walking;
    matrix_weights_main[2][W_OFFSET]+=70 *map_mode_walking;
    
    
    //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;

  }
}

void printModes() {
  Serial.print("Walking: \t"); Serial.println(map_mode_walking);
  Serial.print("RollPitch: \t"); Serial.println(map_mode_rollpitch);
  Serial.print("Sleeping: \t"); Serial.println(map_mode_sleeping);
  Serial.print("Noise: \t"); Serial.println(map_mode_noise);
  Serial.println();
}

void updateServosByWeights(unsigned long millis,float pitch,float roll){
  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_MIN SERVOWEIGHT_UPDATE_INTERVAL*0.5
    #define MILLISADD_MAX SERVOWEIGHT_UPDATE_INTERVAL*2
    millis_add+=constrain(mapfloat(mood.arousal, -0.5,0.5 ,MILLISADD_MIN,MILLISADD_MAX),MILLISADD_MIN,MILLISADD_MAX);


    float vsin=sin((millis_add)/1000.0);
    float vcos=cos((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

      pitch=constrain(pitch,-80,80);
      roll=constrain(roll,-80,80);
      

      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]
          +1.0*matrix_weights_main[servoPosInMatrix][W_OFFSET];
        
      }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 interpolateTo(float value,float aimvalue,float timestep,float speed) 
{
  float result=value;
  if (result<aimvalue){
    result=result+timestep*speed;
    if (result>aimvalue) {
      result=aimvalue;
    }
  }else if (result>aimvalue){
    result=result-timestep*speed;
    if (result<aimvalue) {
      result=aimvalue;
    }
  }
  return result;
}


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;
}

void printMood(){
  Serial.print("A:"); Serial.print(mood.arousal); Serial.print(" V:"); Serial.print(mood.valence);
  Serial.println();
}