#include "mot.h"

#include "pwm.h"

void QUAD_Set(short throt, short x, short y, short z)
{
  //  2   0
  //  1   3
  const unsigned char ESC_UL = 2;  // CW  // M3
  const unsigned char ESC_UR = 0;  // CCW // M1
  const unsigned char ESC_DL = 1;  // CCW // M2
  const unsigned char ESC_DR = 3;  // CW  // M4
  
  
  const unsigned long count = 4;
  
  short pwm[count] {0,0,0,0};
  
  pwm[ESC_UL] += x; pwm[ESC_UR] += x;
  pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
                        
  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
  
  pwm[ESC_UL] -= z; pwm[ESC_UR] += z;
  pwm[ESC_DL] += z; pwm[ESC_DR] -= z;
  
  short min=500, max=-500;
  for(int a=0; a<count; a++)
  {
    if(pwm[a]>500) pwm[a]=500;
    else if(pwm[a]<-500) pwm[a]=-500;
    //---
    if(min>pwm[a]) min=pwm[a];
    if(max<pwm[a]) max=pwm[a];
  }
  
  short up=(throt+max)-1000;
  short down=(throt+min);
  if(up>0) throt-=up;
  if(down<0) throt-=down;
  
  for(int a=0; a<count; a++) pwm[a] += throt+1000;
  
  PWM_SetQuad(pwm, 900, 1500);
}
//------------------------------------------------------------------------------
//tree drone
void QUAD_Set2(short throt, short x, short y, short z)
{
  //  1   2
  //  3   0
  const unsigned char ESC_UL = 1;  // CW  // M3
  const unsigned char ESC_UR = 2;  // CCW // M1
  const unsigned char ESC_DL = 3;  // CCW // M2
  const unsigned char ESC_DR = 0;  // CW  // M4
  
  const unsigned long count = 4;
  
  short pwm[count] {0,0,0,0};
  
  pwm[ESC_UL] += x; pwm[ESC_UR] += x;
  pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
                        
  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
  
  pwm[ESC_UL] -= z; pwm[ESC_UR] += z;
  pwm[ESC_DL] += z; pwm[ESC_DR] -= z;
  
  short min=500, max=-500;
  for(int a=0; a<count; a++)
  {
    if(pwm[a]>500) pwm[a]=500;
    else if(pwm[a]<-500) pwm[a]=-500;
    //---
    if(min>pwm[a]) min=pwm[a];
    if(max<pwm[a]) max=pwm[a];
  }
  
  short up=(throt+max)-1000;
  short down=(throt+min);
  if(up>0) throt-=up;
  if(down<0) throt-=down;
  
  for(int a=0; a<count; a++) pwm[a] += throt+1000;
  
  PWM_SetQuad(pwm, 1100, 2000);
}
//------------------------------------------------------------------------------

void QUAD_Set3(short throt, short x, short y, short z)
{
  //const unsigned char ESC_UL = 0;  // CW  // M3
 // const unsigned char ESC_UR = 3;  // CCW // M1
  //const unsigned char ESC_DL = 2;  // CCW // M2
 // const unsigned char ESC_DR = 1;  // CW  // M4
  
 // const unsigned char ESC_UL = 3;  // CW  // M3
  //const unsigned char ESC_UR = 1;  // CCW // M1
  //const unsigned char ESC_DL = 0;  // CCW // M2
  //const unsigned char ESC_DR = 2;  // CW  // M4
  
  const unsigned char ESC_UL = 2;  // CW  // M3
  const unsigned char ESC_UR = 0;  // CCW // M1
  const unsigned char ESC_DL = 1;  // CCW // M2
  const unsigned char ESC_DR = 3;  // CW  // M4
  
  
  const unsigned long count = 4;
  short pwm[count] {0,0,0,0};
  
  pwm[ESC_UL] += x; pwm[ESC_UR] += x;
  pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
                        
  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
  
  pwm[ESC_UL] -= z; pwm[ESC_UR] += z;
  pwm[ESC_DL] += z; pwm[ESC_DR] -= z;
  
  short min=500, max=-500;
  for(int a=0; a<count; a++)
  {
    if(pwm[a]>500) pwm[a]=500;
    else if(pwm[a]<-500) pwm[a]=-500;
    //---
    if(min>pwm[a]) min=pwm[a];
    if(max<pwm[a]) max=pwm[a];
  }
  
  short up=(throt+max)-1000;
  short down=(throt+min);
  if(up>0) throt-=up;
  if(down<0) throt-=down;
  
  for(int a=0; a<count; a++) pwm[a] += throt+1000;
  
  PWM_SetQuad(pwm, 1100, 2000);
}
//------------------------------------------------------------------------------

void HEXA_Set(short throt, short x, short y, short z)
{
  //    2
  //  1   4
  //  5   0
  //    3
  const unsigned char ESC_UM = 2;  // CW  // M1
  const unsigned char ESC_UL = 1;  // CCW // M5
  const unsigned char ESC_UR = 4;  // CCW // M4
  const unsigned char ESC_DL = 5;  // CW  // M3
  const unsigned char ESC_DR = 0;  // CW  // M6
  const unsigned char ESC_DM = 3;  // CCW // M2
  
  const unsigned long count = 6;
  
  static short pwm[count];
  for(int a=0; a<count; a++) pwm[a] = 0;
  
  pwm[ESC_UL] += x/2; pwm[ESC_UM] += x; pwm[ESC_UR] += x/2;
  pwm[ESC_DL] -= x/2; pwm[ESC_DM] -= x; pwm[ESC_DR] -= x/2;
                        
  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
                        
  pwm[ESC_UL] += z; pwm[ESC_UM] -= z; pwm[ESC_UR] += z;
  pwm[ESC_DL] -= z; pwm[ESC_DM] += z; pwm[ESC_DR] -= z;
  
  
  short min=500, max=-500;
  for(int a=0; a<count; a++)
  {
    if(pwm[a]>500) pwm[a]=500;
    else if(pwm[a]<-500) pwm[a]=-500;
    //---
    if(min>pwm[a]) min=pwm[a];
    if(max<pwm[a]) max=pwm[a];
  }
  
  short up=(throt+max)-1000;
  short down=(throt+min);
  if(up>0) throt-=up;
  if(down<0) throt-=down;
  
  for(int a=0; a<count; a++) pwm[a] += throt+1000;
  
  PWM_SetHexa(pwm, 1100, 1800);
}
//------------------------------------------------------------------------------