#ifndef BEHAVIOUR_H_
#define BEHAVIOUR_H_


#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise
#include "definitions.h"
#include "servo.h"

extern dxl_servo servos[SERVO_COUNT];



#define SERVO_COUNT_MAIN 3
#define SERVO_COUNT_PERIPHERAL 3

#define WEIGHT_COUNT 8 //enter number of weights here for array size


#define WEIGHT_UPDATE_INTERVAL 100
#define SERVOWEIGHT_UPDATE_INTERVAL SERVO_UPDATE_INTERVAL

#define MOOD_UPDATE_INTERVAL 1000.0/50


#define W_PITCH 0
#define W_ROLL 1
#define W_NOISE 2
#define W_NOISESLOW 3
#define W_SIN 4
#define W_COS 5
#define W_LOOKDIRECTION 6
#define W_OFFSET 7


struct Mood{
  float arousal; //-1.0 to 1.0
  float valence; //-1.0 to 1.0
};
extern Mood mood;

const float behaviour_confortable_lift=0.1; // Meters per seond. heightchange below does not affect mood


void initBehaviour();
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);
void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll);
void updateServosByWeights(unsigned long millis,float pitch,float roll);
float interpolateTo(float value,float aimvalue,float timestep,float speed);
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
void printMood();
void printModes();

#endif