/*
 * Copyright (c) 2015-2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 *  ======== Laser.c ========
 */
#include <stddef.h>
#include <math.h>
#include <stdio.h>
#include <strings.h>

/* Driver Header files */
#include <ti/drivers/PWM.h>
#include "sdi_task.h"
/* Example/Board Header files */
#include "Board.h"
#include "SRI_Global.h"
#include "DAC121C085.h"
#include "EEPROM.h"
#include "Laser.h"
#include "PCA9536.h"
#include "Parse.h"
#include "Util.h"

PWM_Handle Laser_pwm = NULL;
static char ftoa_buffer[16] = {0};

void ldcooling_init(void)
{
  DAC121C085_Init();
  ldcooling_set(eeprom_base.eeprom.LD_Temp);
  Control_LaserCooling_En(ON);
  usleep(700*1000); // 700 mS delay
}   /* end ldcooling_init() */


int32_t Laser_Init(void)
{
    PWM_Params params1;

    PWM_Params_init(&params1);
    params1.dutyUnits = PWM_DUTY_COUNTS;
    params1.dutyValue = 0;
    params1.periodUnits = PWM_PERIOD_COUNTS;
    params1.periodValue = LASER_PWM_PERIOD;  // 10 KHz with main clock at 48 MHz
    params1.idleLevel = PWM_IDLE_HIGH;

    Laser_pwm = PWM_open(Board_PWM1, &params1);
    if (Laser_pwm == NULL) {
        /* Laser_pwm did not open */
        return (-1);
    }
    ldcooling_init();
    status.laser_pwr_set = eeprom_base.eeprom.LaserPwrLevels[MAX_LASER_PWRS - 1];
    if (eeprom_base.eeprom.LaserClass == 3)
    {
        GPIO_setOutputEnableDio( MINIRAMAN_INTERLOCK_1 , 1 );
        GPIO_setOutputEnableDio( MINIRAMAN_INTERLOCK_2 , 1 );
        GPIO_clearDio(MINIRAMAN_INTERLOCK_1);
        GPIO_clearDio(MINIRAMAN_INTERLOCK_2);
    }
    else
    {
        GPIO_setOutputEnableDio( MINIRAMAN_INTERLOCK_1 , 0 );
        GPIO_setOutputEnableDio( MINIRAMAN_INTERLOCK_2 , 0 );
    }
    return (0);
}   /* end Laser_Init() */

/* Laser control takes values of 0 - 1000
 * and sets the PWM duty cycle to achieve
 * the desired laser power. */
int32_t Laser_Control(uint16_t drive_level)
{
    int32_t return_val;
    uint32_t duty;

    if (status.safe != TRUE) return (-1);
    else if (drive_level > 1000) return (-1);
    else if (drive_level == 0)
    {
        PWM_stop(Laser_pwm);
        return_val = 0;
    }
    else
    {
        PWM_start(Laser_pwm);
        duty = LASER_PWM_PERIOD - (drive_level * (LASER_PWM_PERIOD/1000));
        return_val = PWM_setDuty(Laser_pwm, duty);
    }
    return (return_val);
}   /* end Laser_Control() */


/* Starting at 10C and ending at 85C for the Epcos thermistor with 10K to 1.5 V ref.
 * Used to convert thermistor voltage to laser temperature. */
const float LaserTempV[] =
{
  9.6784E-01,9.5337E-01,9.3890E-01,9.2442E-01,9.0995E-01,8.9548E-01,8.8086E-01,8.6624E-01,8.5161E-01,8.3699E-01,
  8.3304E-01,8.1643E-01,7.9982E-01,7.8321E-01,7.6661E-01,7.5000E-01,7.3386E-01,7.1772E-01,7.0158E-01,6.8544E-01,
  6.6930E-01,6.5585E-01,6.4240E-01,6.2895E-01,6.1551E-01,5.9261E-01,5.7995E-01,5.6729E-01,5.5463E-01,5.4197E-01,
  5.2133E-01,5.0958E-01,4.9783E-01,4.8608E-01,4.7433E-01,4.5609E-01,4.4530E-01,4.3452E-01,4.2374E-01,4.1296E-01,
  4.3640E-01,4.2660E-01,4.1680E-01,4.0700E-01,3.9720E-01,3.8741E-01,3.7857E-01,3.6973E-01,3.6089E-01,3.5206E-01,
  3.4322E-01,3.3532E-01,3.2742E-01,3.1953E-01,3.1163E-01,3.0373E-01,2.9672E-01,2.8971E-01,2.8270E-01,2.7569E-01,
  2.6868E-01,2.6246E-01,2.5624E-01,2.5002E-01,2.4380E-01,2.3759E-01,2.3209E-01,2.2660E-01,2.2111E-01,2.1561E-01,
  2.1012E-01,2.0528E-01,2.0045E-01,1.9561E-01,1.9078E-01,1.8594E-01
};

/* Uses the ADC reading in microvolts to derive the laser temperature. */

float Laser_Temp_Calc(uint32_t ThermistorMicrovolts)
{
  unsigned int i = 0;
  float VDelta;
  float TDelta;
  float ThermistorV;
  float Temp_value;

  ThermistorV = (float)(ThermistorMicrovolts);
  ThermistorV = ThermistorV/1e6;
  while (LaserTempV[i] > ThermistorV)
  {
    i++;
    if (i > 75) // If the end of the lookup table is reached.
    {
      Temp_value = 100.0;
      return (Temp_value);
    }
  }
  if (i > 0)
  {
    VDelta = LaserTempV[i-1] - LaserTempV[i];
    TDelta = LaserTempV[i-1] - ThermistorV;
    Temp_value = ((float)i + 9 + (TDelta/VDelta));
    return (Temp_value);
  }
  else
  {
    Temp_value = 0.0;
    return (Temp_value);
  }
}   /* end Laser_Temp_Calc() */


 /* Ref for 12 bit DAC is 1.5V from MAX8521.
  * Used to convert from desired laser temperature
  * to DAC counts. */
 const unsigned int LDTempDac[] =
 {
  2642,   2607,   2570,   2530,   2488,   2444,   2409,   2369,   2330,   2288,
  2244,   2209,   2171,   2132,   2090,   2048,   2013,   1973,   1936,   1896,
  1855,   1822,   1786,   1749,   1712,   1672,   1639,   1606,   1571,   1536,
  1498,   1467,   1437,   1406,   1373,   1338,   1311,   1283,   1252,   1223,
  1190,   1166,   1140,   1113,   1085,   1058,   1034,   1010,   986,    961,
  937,    915,    895,    873,    851,    829,    810,    792,    772,    752,
  733,    717,    700,    684,    667,    649,    634,    618,    605,    590
 };

 unsigned int LD_CoolingCalc(float new_temp)
 {
   double temp_index;
   unsigned int index;
   unsigned int lowval;
   unsigned int highval;
   double fraction;
   unsigned int delta;
   unsigned int data_val;

   fraction = modf((double)new_temp, &temp_index);
   index = (unsigned int)temp_index;
   /* This is due to the table starting at 10 degrees */
   index -= 10;

   /* Data Format: MSB--LSB x x x x */
   lowval = LDTempDac[index];
   highval = LDTempDac[index+1];
   delta = lowval - highval;
   delta = (unsigned int)(((double)delta) * fraction);
   data_val = lowval - delta;
   return(data_val);
 }   /* end LD_CoolingCalc() */


/* Uses the desired temperature to calculate the correct DAC setting
 * and then sets the DAC for Laser temperature control. */
void ldcooling_set(float new_temp)
{
  uint16_t dac_val;

  if ((new_temp < 25.0) || (new_temp > 50.0)) return;
  dac_val = LD_CoolingCalc(new_temp);
  DAC121C085_Set(dac_val);
  status.laserTempSet = new_temp;
}   /* end ldcooling_set() */


 /*Check_Interlocks*/
 int check_interlocks(void)
 {
/*   int safeState = 0;

   if (eeprom.LaserClass == 1)
   {
     if((GET_INTERLOCK  ||  GET_INTERLOCK2) || status.RasterStall == TRUE)
     {
       Laser_Control(0);
       Raster_Control(0);
//     image_init();
       status.AcquisitionOK = FALSE;
       safeState = 0; //UNSAFE
     }
     else
     {
       safeState = 1; //SAFE
     }
   }
   else if (eeprom.LaserClass == 3)
   {
     if ( GET_INTERLOCK  || status.RasterStall == TRUE)
     {
         Laser_Control(0);
         Raster_Control(0);
  //     image_init();
         status.AcquisitionOK = FALSE;
         safeState = 0; //UNSAFE
     }
     else
     {
       safeState = 1; //SAFE
     }
   }
   return(safeState);*/
     return (1);
 }


 void ldtemp_cmd(void)
 {
   float ldtemp;

   if (parsed[parsed_tail].token == SET)
   {
     //sscanf(parsed[parsed_tail].string[0], "%f", &ldtemp);
     //ldtemp =   stof(parsed[parsed_tail].string[0]);
     if ((ldtemp > parsed[parsed_tail].limits[0].upper) || (ldtemp < parsed[parsed_tail].limits[0].lower))
     {
       SDITask_Printf("\nLDTEMP_CMD: SET TEMPERATURE OUT OF RANGE (+20 - +60C)");
       return;
     }
     status.laserTempSet = ldtemp;
     ldcooling_set(status.laserTempSet);
     eeprom_base.eeprom.LD_Temp = ldtemp;
     //EEPROM_UpdateChecksum();
   }
   if (parsed[parsed_tail].token == QUERY)
   {
      dtoa(status.laserTempSet,ftoa_buffer,2);
      SDITask_Printf("\n LDTEMP: SET = %s DEGREES", ftoa_buffer);
      dtoa(status.LD_Temp,ftoa_buffer,2);
      SDITask_Printf("\n ACTUAL = %s DEGREES", ftoa_buffer);
      dtoa(MAX_LASER_TEMP,ftoa_buffer,2);
      SDITask_Printf("\n Laser Over Temp Max: %s", ftoa_buffer);
   }
 }   /* end ldtemp_cmd() */


/*
 *   {
    "LASERPWR",             // Control power to the laser #1 (783 nm)
    " Sets or reports the laser status.", //Use: LASERPWR <ON, OFF, CLASSIC, N>.  Sets or reports the laser status.",
    " For laser settings, type: <laserpwr>. To turn laser on/off, type: <laserpwr on/off>.",
    " To set laser power or wavelength, type: <laserpwr [set, wavelength] #>.",
    {
      {TOKEN, "ON", TOK_ON, laserpwr_cmd},
      {TOKEN, "OFF", TOK_OFF, laserpwr_cmd},
      {TOKEN, "SET", SET, laserpwr_cmd},
      {NONE, "", QUERY, laserpwr_cmd},
      {TOKEN, "CLASS", TYPE, laserpwr_cmd},
      {TOKEN, "LEVELS", LEVELS, laserpwr_cmd},
      {TOKEN, "SELECT", SELECT, laserpwr_cmd},
    },
    {{0, 255}, {0, 0}}
  },
 */
 void laserpwr_cmd(void)
 {
   uint16_t laser_pwr_lvl;
   uint16_t temp_dac;
   uint8_t laser_type;

   switch (parsed[parsed_tail].token)
   {
   case TOK_ON:
       Laser_Control(status.laser_pwr_set);
       status.laser_pwr_state = ON;
     break;

   case TOK_OFF:
       Laser_Control(0);
       status.laser_pwr_state = OFF;
     break;

   case SET:
     sscanf(parsed[parsed_tail].string[1], "%d", &laser_pwr_lvl);
     if (laser_pwr_lvl > 1000)
     {
       SDITask_Printf("\n LASERPWR: ERROR, NEW VALUE %d, OUT OF RANGE (0 - 1000)",laser_pwr_lvl);
       return;
     }
     else
     {
       Laser_Control(laser_pwr_lvl);
       status.laser_pwr_set = laser_pwr_lvl;
     }
   break;

   case LEVELS:
    // sscanf(parsed[parsed_tail].string[1], "%s", &levels);
     if ((strcasecmp(parsed[parsed_tail].string[1], "SHOW") == 0) || (strcasecmp(parsed[parsed_tail].string[1], "") == 0))
     {
       SDITask_Printf("\n Laser Levels:");
       for (int i = 1; i < 6; i++)
       {
         if (eeprom_base.eeprom.LaserPwrLevels[i-1] != 0)
         {
           SDITask_Printf("\n %d: %d", i, eeprom_base.eeprom.LaserPwrLevels[i-1]);
         }
         else break;
       }
     }
     else
     {
       for (int i = 1; i <= 5; i++)
       {
         sscanf(parsed[parsed_tail].string[i], "%d", &laser_pwr_lvl);
         if (laser_pwr_lvl != 0)
         {
           if((laser_pwr_lvl < 50) || (laser_pwr_lvl > 1000))
           {
             SDITask_Printf("\n Level[%d] is out of range 50-1000", (i-1));
             return;
           }
           else
           {
               eeprom_base.eeprom.LaserPwrLevels[i-1] = laser_pwr_lvl;
               eeprom_base.eeprom.laserpwrs = i;
             laser_pwr_lvl = 0;
           }
         }
         else
         {
             eeprom_base.eeprom.LaserPwrLevels[i-1] = 0;
         }
       }
  //     EEPROM_UpdateChecksum();
     }
   break;

   case SELECT:
     sscanf(parsed[parsed_tail].string[1], "%d", &laser_pwr_lvl);
     if (laser_pwr_lvl > 5)
     {
       SDITask_Printf("\n Enter Laser Level between 0-5");
     }
     else
     {

       temp_dac = eeprom_base.eeprom.LaserPwrLevels[laser_pwr_lvl - 1];
       if ((temp_dac == 0) && (laser_pwr_lvl != 0)) SDITask_Printf("\nLaser Level not set");
       else if (((temp_dac < 50) || (temp_dac > 800)) && (laser_pwr_lvl != 0))
       {
         SDITask_Printf("\n Laser Level out of safe range, 50-800");
         return;
       }
       else
       {
         if(status.Software == OFF)
           {
             SDITask_Printf("\n Laser Power set to Level: %d = %d", laser_pwr_lvl, eeprom_base.eeprom.LaserPwrLevels[laser_pwr_lvl-1]);
           }
         status.laser_pwr_select = laser_pwr_lvl;
         status.laser_pwr_set = temp_dac;
       }
     }
   break;

   case TYPE:
     sscanf(parsed[parsed_tail].string[1], "%d", &laser_type);
     if (laser_type == 1)
     {
        eeprom_base.eeprom.LaserClass = 1;
        GPIO_setOutputEnableDio(MINIRAMAN_INTERLOCK_1 , 0);
        GPIO_setOutputEnableDio(MINIRAMAN_INTERLOCK_2 , 0);
     }
     else
     {
       eeprom_base.eeprom.LaserClass = 3;
       GPIO_setOutputEnableDio(MINIRAMAN_INTERLOCK_1 , 1);
       GPIO_setOutputEnableDio(MINIRAMAN_INTERLOCK_2 , 1);
       GPIO_clearDio(MINIRAMAN_INTERLOCK_1);
       GPIO_clearDio(MINIRAMAN_INTERLOCK_2);
     }
//    EEPROM_UpdateChecksum();
   break;

   case QUERY:
     if (status.laser_pwr_state == ON) SDITask_Printf("\n LASERPWR: ON     ");
     else SDITask_Printf("\n LASERPWR: OFF     ");
     SDITask_Printf("LASER DAC: %d", status.laser_pwr_set);
     SDITask_Printf("\n Laser Level: %d", status.laser_pwr_select);
     SDITask_Printf("\n Laser Levels: 1: %d, 2: %d, 3: %d, 4: %d, 5: %d", eeprom_base.eeprom.LaserPwrLevels[0], eeprom_base.eeprom.LaserPwrLevels[1],
     eeprom_base.eeprom.LaserPwrLevels[2], eeprom_base.eeprom.LaserPwrLevels[3], eeprom_base.eeprom.LaserPwrLevels[4]);
     if (eeprom_base.eeprom.LaserClass == 1) SDITask_Printf("\n LASER CLASS 1");
     else SDITask_Printf("\n LASER CLASS 3B");
    break;

    }   /* end switch on parsed[tail].token */
 }   /* end laserpwr_cmd() */


/************************************ Done ***********************************************/



