//ピクセルシェーダ
float4 PS(VSOutput input):SV_TARGET
{
    float3 L=normalize(LightDir);
    float3 N=normalize(input.Normal);
    
    float lambertian=saturate(dot(L,N));
    
    float3 V=normalize(-ViewDir);
    
    float3 half_dir=normalize(L+V);
    float spec_angle=saturate(dot(half_dir,N));
    float specular=pow(spec_angle,Power);
    
    float4 Kt=txDiffuse.Sample(input.Texture);
    float4 Kd=LightDiffuse*Diffuse*Kt;
    float4 Ks=LightSpecular*float4(Specular,1.0f);
    
    float4 color_linear=LightAmbient*Kd;
    
    float2 texel=float2(
    input.Shadow.x/input.Shadow.w*0.5f+0.5f,
                        input.Shadow.y/input.Shadow.w*-0.5f+0.5f);
    if(texel.x<0||texel.x>1||texel.y<0||texel.y>1){
        color_linear+=lambertian*Kd+specular*Ks;
    }
    else{
        float shadow=txShadow.Sample(samLinear,texel).r;
        if(shadow>=input.Shadow.z/input.Shadow.w-0.0001f){
            color_linear+=lambertian*Kd+specular*Ks;
        }
    }
    return saturate(color_linear);
}

bool ofxColorPicker_<ColorType>::mouseUpdate(ofMouseEventArgs& mouse){
	if(rectBackground.inside(mouse)){
		switch (state) {
			case ChangingScale:{
				int relY = mouse.y - rectColorScaleBar.y;
				float scale = 1.f - saturate(relY / rectColorScaleBar.height);
				setColorScale(scale);
				
				setNeedsRedraw();
				break;
			}
			case ChangingWheel:{
				auto p = mouse - rectColorWheel.position.xy();
				auto pc = getPolarCoordinate(p, colorWheelRadius);
				
				colorAngle	= pc.angle;
				colorRadius	= saturate(pc.radius);
				
				bSettingColor = true;
				color = getCircularColor<ColorType>(colorAngle, colorRadius, colorScale);
				bSettingColor = false;
				setNeedsRedraw();
				break;
			}
			default: return true;
		}
		return true;
	}
	return false;
}

double* uavAutopilot(UavStates* uav,autoPilot* pid, double V,double h,double theta,double q,double phi,double p,double xiDV,double xiDh,double xiDtheta,double xiDphi,double phid)
{
//// The Autopilot
// issues control commands

double deg2rad = pi/180;
double rad2deg = 180/pi;

// velocity hold
double dV = uav->V0-V;
double delta_t = saturate(dV*pid->kVP+xiDV*pid->kVI+1.0075,0.8,2);
// delta_t = saturate(dV*pid->kVP+xiDV*pid->kVI,0.8,2);

// altitude hold
double dh = uav->h0-h;
double theta_error = (dh*pid->khP+xiDh*pid->khI-(theta-uav->theta0)*rad2deg)*pid->kthetaP+xiDtheta*pid->kthetaI;
double q_error = theta_error-q*rad2deg*pid->kqP;
double delta_e = saturate(q_error+0.0018*rad2deg,-40,40)*deg2rad;

// attitude hold
double dphi = (phid - phi)*rad2deg;
double p_error = dphi*pid->kphiP+xiDphi*pid->kphiI-p*rad2deg*pid->kpP;
double delta_a = saturate(p_error,-40,40)*deg2rad;

double delta_r = 0;

double arr[4] = {delta_t,delta_e,delta_a,delta_r};

return arr;//{delta_t,delta_e,delta_a,delta_r};

}

void lit_point_spec_range(\n\
	float3 in_pos,\n\
	float3 in_nor,\n\
	float3 cam_vec,\n\
	float shin,\n\
	float3 l_pos,\n\
	float3 l_diff,\n\
	float3 l_spec,\n\
	float2 l_range,\n\
	inout float3 out_diff,\n\
	inout float3 out_spec\n\
	)\n\
{\n\
	float diff_int, spec_int;\n\
	float3 dir;\n\
	float3 half_vec;\n\
\n\
	diff_int = dot(in_nor, dir = normalize(l_pos - in_pos));\n\
	if(diff_int > 0.0)\n\
	{\n\
		float att = 1 - saturate(distance(l_pos, in_pos) * l_range[0]);\n\
\n\
		out_diff += l_diff * diff_int * att;\n\
		half_vec = normalize(cam_vec + dir);\n\
		spec_int = dot(in_nor, half_vec);\n\
		out_spec += l_spec * pow(saturate(spec_int), shin) * att;\n\
	}\n\
}\n\

 void setPixel(const int x, const int y, const float r, const float g, const float b) {
   assert(format() == VPX_IMG_FMT_RGB24);
   assert(x>=0 && x<width() && y>=0 && y<height());
   uint8_t* buf = buffer();
   buf[(y*width()+x)*3+0] = uint8_t(saturate(r) * 255.0);
   buf[(y*width()+x)*3+1] = uint8_t(saturate(g) * 255.0);
   buf[(y*width()+x)*3+2] = uint8_t(saturate(b) * 255.0);
 }

coord_type fi_mul(coord_type op1, coord_type op2)
{
    coord_type tmp_op1 = saturate(op1);
    coord_type tmp_op2 = saturate(op2);
    coord_type result = tmp_op1*tmp_op2;
    result = result >> FRACTIONAL_BITS;
    return result;
}

// get the color
unsigned int RGBSpectrum::GetColor() const
{
	unsigned int color = 0;

	color |= ((unsigned char)(255.0f*saturate(m_r)))<<16;
	color |= ((unsigned char)(255.0f*saturate(m_g)))<<8;
	color |= ((unsigned char)(255.0f*saturate(m_b)))<<0;

	return color;
}

// ********************************************************************************************************\n\
float4 PSMainNoTexture( PS_INPUT_NO_TEX input ) : SV_Target\n\
{\n\
    float3 normal       = normalize(input.Norm);\n\
    float  nDotL = saturate( dot( normal, -normalize(LightDirection.xyz) ) );\n\
    float3 eyeDirection     = normalize(EyePosition.xyz - input.Position);\n\
    float3 HalfVector       = normalize( eyeDirection + (-LightDirection.xyz) );\n\
    float  nDotH            = saturate( dot(normal, HalfVector) );\n\
    float3 specular         = 0.3f * pow(nDotH, 50.0f );\n\
    return float4( (nDotL + specular).xxx, 1.0f);\n\
}\n\

// inner product of p1 and p2
void dot_product_fi(data_type p1,data_type p2, coord_type *r)
{
    coord_type tmp = 0;
    for (uint d=0;d<D;d++) {
        coord_type tmp_op1 = saturate(p1.value[d]);
        coord_type tmp_op2 = saturate(p2.value[d]);
        coord_type tmp_mul = tmp_op1*tmp_op2;
        tmp += tmp_mul;       
    }
    *r = (tmp);    
}

// ********************************************************************************************************\n\
float4 PSMainNoTexture( PS_INPUT_NO_TEX input ) : SV_Target\n\
{\n\
    float3 lightUv = input.LightUv.xyz / input.LightUv.w;\n\
    float2 uv = lightUv.xy * 0.5f + 0.5f;\n\
    float3 eyeDirection = normalize(input.Position - EyePosition.xyz);\n\
    float3 normal       = normalize(input.Norm);\n\
    float  nDotL = saturate( dot( normal, -normalize(LightDirection.xyz) ) );\n\
    float3 reflection   = reflect( eyeDirection, normal );\n\
    float  rDotL        = saturate(dot( reflection, -LightDirection.xyz ));\n\
    float  specular     = 0.2f * pow( rDotL, 4.0f );\n\
    return float4( (nDotL + specular).xxx, 1.0f);\n\
}\n\

int ip_addr_cmp(struct ip_addr const *a, struct ip_addr const *b)
{
    if (a->family < b->family) return -1;
    else if (a->family > b->family) return 1;
    else switch (a->family) {
        case AF_INET:
            return saturate(memcmp(&a->u.v4, &b->u.v4, sizeof(a->u.v4)));
        case AF_INET6:
            return saturate(memcmp(&a->u.v6, &b->u.v6, sizeof(a->u.v6)));
        }
    FAIL("Invalid IP family (%d)", a->family);
    return -1;
}

// ********************************************************************************************************\n\
float4 PSMain( PS_INPUT input ) : SV_Target\n\
{\n\
    float3 normal         = normalize(input.Norm);\n\
    float  nDotL          = saturate( dot( normal, -LightDirection ) );\n\
    float3 eyeDirection     = normalize(EyePosition.xyz - input.Position);\n\
    float3 HalfVector       = normalize( eyeDirection + (-LightDirection.xyz) );\n\
    float  nDotH            = saturate( dot(normal, HalfVector) );\n\
    float3 specular         = 0.3f * pow(nDotH, 50.0f );\n\
    float4 diffuseTexture = TEXTURE0.Sample( SAMPLER0, input.Uv );\n\
    float ambient = 0.05;\n\
    float3 result = (nDotL+ambient) * diffuseTexture + specular;\n\
    return float4( result, 1.0f );\n\
}\n\

// inner product of p1 and p2
void dot_product_fi_mixed(data_type_short p1,data_type p2, coord_type *r)
{
    data_type tmp_p1 = conv_short_to_long(p1);
    coord_type tmp = 0;
    for (uint d=0;d<D;d++) {
            
        coord_type tmp_op1 = saturate(tmp_p1.value[d]);
        coord_type tmp_op2 = saturate(p2.value[d]);           
        
        coord_type tmp_mul = tmp_op1*tmp_op2;
        tmp += tmp_mul;       
    }
    *r = (tmp);    
}

static void runAddingLowPass(LADSPA_Handle instance, unsigned long sample_count) {

	LowPass *plugin_data = (LowPass*)instance;
	LADSPA_Data run_adding_gain = plugin_data->run_adding_gain;

	/*cutoff (Hz)*/
	const LADSPA_Data cutoff = *(plugin_data->m_pfcutoff);
	/* Audio input: input1*/
	const LADSPA_Data * const input1 = plugin_data->m_pfinput1;

	/* Audio input: input2*/
	const LADSPA_Data * const input2 = plugin_data->m_pfinput2;

	/* Audio input: output1*/
	LADSPA_Data * const output1 = plugin_data->m_pfoutput1;

	/* Audio input: output2*/
	LADSPA_Data * const output2 = plugin_data->m_pfoutput2;

	// Parameters last value:
	float last_cutoff = plugin_data->last_cutoff;

	// States:
	float lp2 = plugin_data->lp2;
	float lp1 = plugin_data->lp1;
	float lambda = plugin_data->lambda;

	if (cutoff != last_cutoff) {

      // cutoff and samplerate are both in Hertz
      lambda = exp(- cutoff / XSPIF_GET_SAMPLE_RATE()); 
      
		plugin_data->last_cutoff = cutoff;
	}


// Here is the DSP algorithm:
	{

    for(int i=0;i < XSPIF_GET_VECTOR_SIZE();i++)
    {
         // in and out names are derived from the label in the pin declaration
         lp1 = (1.f-lambda)*input1[i] + lambda*lp1;
         lp2 = (1.f-lambda)*input2[i] + lambda*lp2;
	 XSPIF_WRITE_SAMPLE(output1, i, saturate(lp1));
	 XSPIF_WRITE_SAMPLE(output2, i, saturate(lp2));  
    }
    	
}
}

void CServer::update_position(u32 robot_idx)
{
    u32 i, j;

    float position_new[ROBOT_SPACE_DIMENSION];
    float tmp_dist = ROBOT_SPACE_DIMENSION;
    std::vector<u32> colisions_idx;

    i32 wall_idx = -1;

    for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
        position_new[i] = robots[robot_idx].position[i] + 5.0*robots[robot_idx].d[i]*dt*0.001 + 0.001*rnd_();

    for (j = 0; j < robots.size(); j++)
        //if ( (j != robot_idx) && (robots[j].type&ROBOT_SOLID_FLAG) )
        if ( (j != robot_idx) && (robots[j].type&ROBOT_STRONG_SOLID_FLAG) )
        {
            tmp_dist = vect_distance(position_new, robots[j].position, ROBOT_SPACE_DIMENSION);

            if (tmp_dist < colision_distance)
            {
                colisions_idx.push_back(j);
                if (robots[j].type&ROBOT_STRONG_SOLID_FLAG)
                    wall_idx = j;
            }
        }


    if (colisions_idx.size() == 0)
        //there is no colision
        for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
            robots[robot_idx].position[i] = saturate(position_new[i], -position_max[i], position_max[i]);
    else
    {
        for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
        {
            float tmp = 0.0;
            if (wall_idx == -1)
                tmp = -5.0*robots[robot_idx].d[i]*dt*0.001 + 0.001*rnd_();
            else
            {
                tmp = 0.1*(robots[robot_idx].position[i] - robots[wall_idx].position[i] + 0.001*rnd_());
            }

            robots[robot_idx].position[i] = saturate(robots[robot_idx].position[i] + tmp, -position_max[i], position_max[i]);
        }
    }
}

static int16_t gsm_sub (
int16_t a,
int16_t b)
{
        int32_t diff = (int32_t)a - (int32_t)b;
        return saturate(diff);
}

// ----------------------------------------------------------------------------------------------
Eigen::MatrixXd JacoIKSolver::desiredAngles(geometry_msgs::Pose p_in, JacoAngles JAngle)
{
	Eigen::MatrixXd JAngle_out = Eigen::MatrixXd::Zero(1,6);
	
	KDL::JntArray q_out, joint_in;
	KDL::Frame f_in;
	tf::PoseMsgToKDL(p_in,f_in);  
	
	joint_in.resize(6);
	joint_in(0) = ( JAngle.Actuator1 - 180.0 ) * DTR;
	joint_in(1) = ( JAngle.Actuator2 - 270.0 ) * DTR;
	joint_in(2) = ( JAngle.Actuator3 - 90.0  ) * DTR;
	joint_in(3) = ( JAngle.Actuator4 - 180.0 ) * DTR;
	joint_in(4) = ( JAngle.Actuator5 - 180.0 ) * DTR;
	joint_in(5) = ( JAngle.Actuator6 - 270.0 ) * DTR;
	
    ik_solver_->CartToJnt(joint_in, f_in, q_out);
    
    for (int i = 0; i < 6; i++){
		JAngle_out(0,i)= saturate(q_out(i) * RTD) ;
	}
    
    return JAngle_out;
    
}

void effectType(const PassRefPtr<CanvasPixelArray>& srcPixelArray, PassRefPtr<ImageData>& imageData, const Vector<float>& values)
{
    for (unsigned pixelOffset = 0; pixelOffset < srcPixelArray->length(); pixelOffset++) {
        unsigned pixelByteOffset = pixelOffset * 4;

        unsigned char r = 0, g = 0, b = 0, a = 0;
        srcPixelArray->get(pixelByteOffset, r);
        srcPixelArray->get(pixelByteOffset + 1, g);
        srcPixelArray->get(pixelByteOffset + 2, b);
        srcPixelArray->get(pixelByteOffset + 3, a);

        double red = r, green = g, blue = b, alpha = a;
        
        switch (filterType) {
            case FECOLORMATRIX_TYPE_MATRIX:
                matrix(red, green, blue, alpha, values);
                break;
            case FECOLORMATRIX_TYPE_SATURATE: 
                saturate(red, green, blue, values[0]);
                break;
            case FECOLORMATRIX_TYPE_HUEROTATE:
                huerotate(red, green, blue, values[0]);
                break;
            case FECOLORMATRIX_TYPE_LUMINANCETOALPHA:
                luminance(red, green, blue, alpha);
                break;
        }

        imageData->data()->set(pixelByteOffset, red);
        imageData->data()->set(pixelByteOffset + 1, green);
        imageData->data()->set(pixelByteOffset + 2, blue);
        imageData->data()->set(pixelByteOffset + 3, alpha);
    }
}

void lit_spot_range(\n\
	float3 in_pos,\n\
	float3 in_nor,\n\
	float3 l_pos,\n\
	float3 l_dir,\n\
	float inner_cone,\n\
	float outer_cone,\n\
	float3 l_diff,\n\
	float2 l_range,\n\
	inout float3 out_diff\n\
	)\n\
{\n\
	float diff_int, angle, spot_eff;\n\
	float3 dir;\n\
\n\
	diff_int = dot(in_nor, dir = -l_dir);\n\
	if(diff_int > 0.0)\n\
	{\n\
		angle = dot(normalize(in_pos - l_pos), l_dir);\n\
		if(angle > outer_cone)\n\
		{\n\
			float att = 1 - saturate(distance(in_pos, l_pos) * l_range[0]);\n\
\n\
			spot_eff = smoothstep(outer_cone, inner_cone, angle);\n\
			out_diff += l_diff * diff_int * spot_eff * att;\n\
		}\n\
	}\n\
}\n\

void lit_spot_spec(\n\
	float3 in_pos,\n\
	float3 in_nor,\n\
	float3 cam_vec,\n\
	float shin,\n\
	float3 l_pos,\n\
	float3 l_dir,\n\
	float inner_cone,\n\
	float outer_cone,\n\
	float3 l_diff,\n\
	float3 l_spec,\n\
	inout float3 out_diff,\n\
	inout float3 out_spec\n\
	)\n\
{\n\
	float diff_int, spec_int, angle, spot_eff;\n\
	float3 dir;\n\
	float3 half_vec;\n\
\n\
	diff_int = dot(in_nor, dir = -l_dir);\n\
	if(diff_int > 0.0)\n\
	{\n\
		angle = dot(normalize(in_pos - l_pos), l_dir);\n\
		spot_eff = smoothstep(outer_cone, inner_cone, angle);\n\
		out_diff += l_diff * diff_int * spot_eff;\n\
		half_vec = normalize(cam_vec + dir);\n\
		spec_int = dot(in_nor, half_vec);\n\
		out_spec += l_spec * pow(saturate(spec_int), shin);\n\
	}\n\
}\n\